|
From: <rob...@us...> - 2008-10-20 19:51:51
|
Revision: 5560
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5560&view=rev
Author: rob_wheeler
Date: 2008-10-20 19:51:45 +0000 (Mon, 20 Oct 2008)
Log Message:
-----------
- Add flags to indicate if the last calibration settings are valid.
- Correct the sense of rising and falling edges of the calibration sensor
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/scripts/listen.py
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/robot_msgs/msg/ActuatorState.msg
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-20 19:38:01 UTC (rev 5559)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2008-10-20 19:51:45 UTC (rev 5560)
@@ -110,12 +110,12 @@
state_ = BEGINNING;
break;
case BEGINNING:
- original_switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+ original_switch_state_ = actuator_->state_.calibration_reading_;
cc_.steer_velocity_ = (original_switch_state_ ? search_velocity_ : -search_velocity_);
state_ = MOVING;
break;
case MOVING: {
- bool switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+ bool switch_state_ = actuator_->state_.calibration_reading_;
if (switch_state_ != original_switch_state_)
{
Actuator a;
@@ -126,7 +126,7 @@
fake_j.push_back(&j);
// Where was the joint when the optical switch triggered?
- if (original_switch_state_ == true)
+ if (switch_state_ == true)
fake_a[0]->state_.position_ = actuator_->state_.last_calibration_rising_edge_;
else
fake_a[0]->state_.position_ = actuator_->state_.last_calibration_falling_edge_;
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-20 19:38:01 UTC (rev 5559)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-20 19:51:45 UTC (rev 5560)
@@ -122,12 +122,12 @@
state_ = BEGINNING;
break;
case BEGINNING:
- original_switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+ original_switch_state_ = actuator_->state_.calibration_reading_;
vc_.setCommand(original_switch_state_ ? search_velocity_ : -search_velocity_);
state_ = MOVING;
break;
case MOVING: {
- bool switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+ bool switch_state_ = actuator_->state_.calibration_reading_;
if (switch_state_ != original_switch_state_)
{
Actuator a;
@@ -138,7 +138,7 @@
fake_j.push_back(&j);
// Where was the joint when the optical switch triggered?
- if (original_switch_state_ == true)
+ if (switch_state_ == true)
fake_a[0]->state_.position_ = actuator_->state_.last_calibration_rising_edge_;
else
fake_a[0]->state_.position_ = actuator_->state_.last_calibration_falling_edge_;
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-20 19:38:01 UTC (rev 5559)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h 2008-10-20 19:51:45 UTC (rev 5560)
@@ -175,8 +175,8 @@
uint16_t num_encoder_errors_;
uint8_t encoder_status_;
uint8_t calibration_reading_;
+ int32_t last_calibration_falling_edge_;
int32_t last_calibration_rising_edge_;
- int32_t last_calibration_falling_edge_;
uint16_t board_temperature_;
uint16_t bridge_temperature_;
uint16_t supply_voltage_;
@@ -255,7 +255,7 @@
enum
{
LIMIT_SENSOR_0_STATE = 1, LIMIT_SENSOR_1_STATE = 2,
- LIMIT_ON_TO_OFF = 4, LIMIT_OFF_TO_ON = 8
+ LIMIT_OFF_TO_ON = 4, LIMIT_ON_TO_OFF = 8
};
// Board configuration parameters
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-20 19:38:01 UTC (rev 5559)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-20 19:51:45 UTC (rev 5560)
@@ -341,6 +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.calibration_rising_edge_valid_ = this_status->calibration_reading_ & LIMIT_OFF_TO_ON;
+ state.calibration_falling_edge_valid_ = this_status->calibration_reading_ & LIMIT_ON_TO_OFF;
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;
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-20 19:38:01 UTC (rev 5559)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-10-20 19:51:45 UTC (rev 5560)
@@ -42,6 +42,14 @@
class ActuatorState{
public:
ActuatorState() :
+ timestamp_(0),
+ encoder_count_(0),
+ position_(0),
+ encoder_velocity_(0),
+ velocity_(0),
+ calibration_reading_(0),
+ calibration_rising_edge_valid_(0),
+ calibration_falling_edge_valid_(0),
last_calibration_rising_edge_(0),
last_calibration_falling_edge_(0),
is_enabled_(0),
@@ -52,6 +60,7 @@
last_requested_effort_(0),
last_commanded_effort_(0),
last_measured_effort_(0),
+ motor_voltage_(0),
num_encoder_errors_(0),
zero_offset_(0)
{}
@@ -63,6 +72,8 @@
double velocity_;
bool calibration_reading_;
+ bool calibration_rising_edge_valid_;
+ bool calibration_falling_edge_valid_;
double last_calibration_rising_edge_;
double last_calibration_falling_edge_;
Modified: pkg/trunk/mechanism/mechanism_control/scripts/listen.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2008-10-20 19:38:01 UTC (rev 5559)
+++ pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2008-10-20 19:51:45 UTC (rev 5560)
@@ -33,8 +33,10 @@
print " encoder_velocity: %.4f" % a.encoder_velocity
print " velocity: %.4f" % a.velocity
print " calibration_reading: %d" % a.calibration_reading
- print " last_calibration_rising_edge: %d" % a.last_calibration_rising_edge
- print " last_calibration_falling_edge: %d" % a.last_calibration_falling_edge
+ print " calibration_rising_edge_valid: %d" % a.calibration_rising_edge_valid
+ print " calibration_falling_edge_valid: %d" % a.calibration_falling_edge_valid
+ print " last_calibration_rising_edge: %.4f" % a.last_calibration_rising_edge
+ print " last_calibration_falling_edge: %.4f" % 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-20 19:38:01 UTC (rev 5559)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-10-20 19:51:45 UTC (rev 5560)
@@ -261,6 +261,8 @@
out->encoder_velocity = in->encoder_velocity_;
out->velocity = in->velocity_;
out->calibration_reading = in->calibration_reading_;
+ out->calibration_rising_edge_valid = in->calibration_rising_edge_valid_;
+ out->calibration_falling_edge_valid = in->calibration_falling_edge_valid_;
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_;
Modified: pkg/trunk/robot_msgs/msg/ActuatorState.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/ActuatorState.msg 2008-10-20 19:38:01 UTC (rev 5559)
+++ pkg/trunk/robot_msgs/msg/ActuatorState.msg 2008-10-20 19:51:45 UTC (rev 5560)
@@ -5,6 +5,8 @@
float64 encoder_velocity
float64 velocity
byte calibration_reading
+byte calibration_rising_edge_valid
+byte calibration_falling_edge_valid
float64 last_calibration_rising_edge
float64 last_calibration_falling_edge
byte is_enabled
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-10-22 20:22:31
|
Revision: 5661
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5661&view=rev
Author: jfaustwg
Date: 2008-10-22 20:22:26 +0000 (Wed, 22 Oct 2008)
Log Message:
-----------
Switching everything (hopefully) in personalrobots to use rosconsole instead of ros::node::log
Modified Paths:
--------------
pkg/trunk/audio/audio_capture/audio_capture.cpp
pkg/trunk/audio/audio_capture/audio_clip_writer.cpp
pkg/trunk/curltest/curlnode.cpp
pkg/trunk/deprecated/etherdrive_old/src/base_node/base_node.cpp
pkg/trunk/deprecated/etherdrive_old/src/etherdrive_node/etherdrive_node.cpp
pkg/trunk/deprecated/etherdrive_old/src/etherdrive_node_new/etherdrive_node.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_ptz/axis_ptz.cpp
pkg/trunk/drivers/cam/cv_cam/cv_cam.cpp
pkg/trunk/drivers/cam/elphel_cam/src/elphel_cam/elphel_cam.cpp
pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp
pkg/trunk/drivers/input/joy/joy.cpp
pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
pkg/trunk/drivers/robot/segway_apox/segway.cpp
pkg/trunk/nav/teleop_base/teleop_base.cpp
pkg/trunk/nav/teleop_base/teleop_head.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
pkg/trunk/vision/cv_movie_streamer/cv_movie_streamer.cpp
Modified: pkg/trunk/audio/audio_capture/audio_capture.cpp
===================================================================
--- pkg/trunk/audio/audio_capture/audio_capture.cpp 2008-10-22 19:46:56 UTC (rev 5660)
+++ pkg/trunk/audio/audio_capture/audio_capture.cpp 2008-10-22 20:22:26 UTC (rev 5661)
@@ -3,27 +3,27 @@
//
// Copyright (C) 2008, Morgan Quigley
//
-// Redistribution and use in source and binary forms, with or without
+// 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,
+// * 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
+// * 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
+// * 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
+//
+// 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.
//////////////////////////////////////////////////////////////////////////////
@@ -40,7 +40,7 @@
//#define GRATUITOUS_DEBUGING
static int audio_cb(const void *input, void *output,
- unsigned long frame_count,
+ unsigned long frame_count,
const PaStreamCallbackTimeInfo *time_info,
PaStreamCallbackFlags status, void *user_data)
{
@@ -86,31 +86,46 @@
PaError err;
ROS_DEBUG("initializing portaudio");
if (Pa_Initialize() != paNoError)
+ {
ROS_FATAL("unable to initialize portaudio");
+ ROS_BREAK();
+ }
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,
+ 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_BREAK();
+ }
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");
+ ROS_BREAK();
+ }
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_BREAK();
+ }
ROS_DEBUG("shutting down portaudio");
if (Pa_Terminate() != paNoError)
+ {
ROS_FATAL("unable to close portaudio");
+ ROS_BREAK();
+ }
ros::fini();
return 0;
}
Modified: pkg/trunk/audio/audio_capture/audio_clip_writer.cpp
===================================================================
--- pkg/trunk/audio/audio_capture/audio_clip_writer.cpp 2008-10-22 19:46:56 UTC (rev 5660)
+++ pkg/trunk/audio/audio_capture/audio_clip_writer.cpp 2008-10-22 20:22:26 UTC (rev 5661)
@@ -3,27 +3,27 @@
//
// Copyright (C) 2008, Morgan Quigley
//
-// Redistribution and use in source and binary forms, with or without
+// 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,
+// * 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
+// * 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
+// * 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
+//
+// 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.
//////////////////////////////////////////////////////////////////////////////
@@ -54,7 +54,10 @@
void audio_cb()
{
if (audio_msg.num_channels != 1)
- ROS_FATAL("audio_clip_writer can only handle single-channel audio.");
+ {
+ ROS_FATAL("audio_clip_writer can only handle single-channel audio.");
+ ROS_BREAK();
+ }
for (size_t i = 0; i < audio_msg.samples.size(); i++)
{
audio_clock++;
@@ -67,7 +70,7 @@
// compute RMS power
float sum_squares = 0;
int win_idx = 0;
- for (deque<float>::iterator j = window.begin(); j != window.end();
+ 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());
@@ -84,7 +87,7 @@
clip_state = CLIP_START;
clip.clear();
clip.reserve(window.size());
- for (deque<float>::iterator j = window.begin();
+ for (deque<float>::iterator j = window.begin();
j != window.end(); ++j)
clip.push_back(*j);
}
Modified: pkg/trunk/curltest/curlnode.cpp
===================================================================
--- pkg/trunk/curltest/curlnode.cpp 2008-10-22 19:46:56 UTC (rev 5660)
+++ pkg/trunk/curltest/curlnode.cpp 2008-10-22 20:22:26 UTC (rev 5661)
@@ -4,27 +4,27 @@
//
// Copyright (C) 2008, Morgan Quigley
//
-// Redistribution and use in source and binary forms, with or without
+// 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,
+// * 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
+// * 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
+// * 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
+//
+// 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"
@@ -49,9 +49,9 @@
}
virtual ~CurlNode()
- {
- if (curl)
- delete curl;
+ {
+ if (curl)
+ delete curl;
}
bool spin()
@@ -59,7 +59,7 @@
while (ok())
{
if (curl->get())
- log(ros::ERROR,"Get failed.");
+ ROS_ERROR("Get failed.");
publish("chatter", msg_out);
}
Modified: pkg/trunk/deprecated/etherdrive_old/src/base_node/base_node.cpp
===================================================================
--- pkg/trunk/deprecated/etherdrive_old/src/base_node/base_node.cpp 2008-10-22 19:46:56 UTC (rev 5660)
+++ pkg/trunk/deprecated/etherdrive_old/src/base_node/base_node.cpp 2008-10-22 20:22:26 UTC (rev 5661)
@@ -4,27 +4,27 @@
//
// Copyright (C) 2008, Jimmy Sastra
//
-// Redistribution and use in source and binary forms, with or without
+// 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,
+// * 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
+// * 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
+// * 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
+//
+// 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"
@@ -106,21 +106,21 @@
for (int i = 0; i < 6; i++)
{
// setting gains on controls on ethercard drive
- edLeft->set_gain(i,'P',0);
+ edLeft->set_gain(i,'P',0);
edLeft->set_gain(i,'I',10);
- edLeft->set_gain(i,'D',0);
- edLeft->set_gain(i,'W',100);
+ edLeft->set_gain(i,'D',0);
+ edLeft->set_gain(i,'W',100);
edLeft->set_gain(i,'M',1004);
- edLeft->set_gain(i,'Z',1);
- edRight->set_gain(i,'P',0);
+ edLeft->set_gain(i,'Z',1);
+ edRight->set_gain(i,'P',0);
edRight->set_gain(i,'I',10);
- edRight->set_gain(i,'D',0);
- edRight->set_gain(i,'W',100);
+ edRight->set_gain(i,'D',0);
+ edRight->set_gain(i,'W',100);
edRight->set_gain(i,'M',1004);
- edRight->set_gain(i,'Z',1);
+ edRight->set_gain(i,'Z',1);
}
-
-
+
+
edLeft->set_control_mode(1); // 0=voltage, 1=current control
edRight->set_control_mode(1); // 0=voltage, 1=current control
@@ -133,17 +133,17 @@
}
void mot_callback()
- {
+ {
//printf("received motor 3 command %f\n", mot_cmd[3].val);
}
virtual ~EtherDrive_Node()
- {
+ {
if (edRight)
- {
+ {
printf("deleting edRight\n");
- delete edRight;
- }
+ delete edRight;
+ }
if (edLeft)
{
printf("deleting edLeft\n");
@@ -187,7 +187,7 @@
{ // turret motors, position control
desPos[i] = float(mot_cmd[i].cmd)/360*90000;
if(i < 6) posError[i] = desPos[i] - float(edLeft->get_enc(i));
- else posError[i] = desPos[i] - float(edRight->get_enc(i-6));
+ else posError[i] = desPos[i] - float(edRight->get_enc(i-6));
if(i==11) printf("posError: %f, despos: %f, enc: %i\n", posError[i], desPos[i], edRight->get_enc(i-6));
tmp_mot_cmd[i] = int(k_p[i]*posError[i]) + int(k_d[i]*(posError[i]-lastPosError[i])/0.001) + int(k_i[i]*intPosError[i]);
@@ -196,16 +196,16 @@
intPosError[i] += posError[i];
}
else
- { // wheels, velocity control
-
+ { // wheels, velocity control
+
if(i<6) currEncPos[i] = edLeft->get_enc(i);
else currEncPos[i] = edRight->get_enc(i-6);
curVel = float(currEncPos[i] - lastEncPos[i])/9000*0.48;
desVel = mot_cmd[i].cmd; // enc units/msec. should be (wheel rev/s)
if(i == 0| i ==1) tmp_mot_cmd[i] = int(k_p[i]*(desVel - curVel) + k_cross[i]*tmp_mot_cmd[2]);
- else if(i == 3| i ==4) tmp_mot_cmd[i] = int(k_p[i]*(desVel - curVel) + k_cross[i]*tmp_mot_cmd[5]);
- else if(i == 6| i ==7) tmp_mot_cmd[i] = int(k_p[i]*(desVel - curVel) + k_cross[i]*tmp_mot_cmd[8]);
- else if(i == 9| i ==10) tmp_mot_cmd[i] = int(k_p[i]*(desVel - curVel) + k_cross[i]*tmp_mot_cmd[11]);
+ else if(i == 3| i ==4) tmp_mot_cmd[i] = int(k_p[i]*(desVel - curVel) + k_cross[i]*tmp_mot_cmd[5]);
+ else if(i == 6| i ==7) tmp_mot_cmd[i] = int(k_p[i]*(desVel - curVel) + k_cross[i]*tmp_mot_cmd[8]);
+ else if(i == 9| i ==10) tmp_mot_cmd[i] = int(k_p[i]*(desVel - curVel) + k_cross[i]*tmp_mot_cmd[11]);
lastEncPos[i] = currEncPos[i];
}
@@ -221,7 +221,7 @@
tmp_mot_cmd[i] = -3000;
// printf("Max -ve current motor %i \n", i);
}
-
+
}
// edLeft->drive(6,tmp_mot_cmd);
@@ -229,7 +229,7 @@
{
tmp_mot_cmd_left[i] = tmp_mot_cmd[i];
tmp_mot_cmd_right[i] = tmp_mot_cmd[i+6];
- }
+ }
edLeft->drive(6,tmp_mot_cmd_left);
edRight->drive(6,tmp_mot_cmd_right);
// int footmp[6] = {500,500,0,500,500,0};
@@ -247,7 +247,7 @@
return false;
}
-
+
for (int i = 0; i < 6; i++)
{
mot[i].pos_valid = true;
@@ -272,7 +272,7 @@
usleep(1000);
if (!a.do_tick())
{
- a.log(ros::ERROR,"Etherdrive tick failed.");
+ ROS_ERROR("Etherdrive tick failed.");
break;
}
}
Modified: pkg/trunk/deprecated/etherdrive_old/src/etherdrive_node/etherdrive_node.cpp
===================================================================
--- pkg/trunk/deprecated/etherdrive_old/src/etherdrive_node/etherdrive_node.cpp 2008-10-22 19:46:56 UTC (rev 5660)
+++ pkg/trunk/deprecated/etherdrive_old/src/etherdrive_node/etherdrive_node.cpp 2008-10-22 20:22:26 UTC (rev 5661)
@@ -4,27 +4,27 @@
//
// Copyright (C) 2008, Morgan Quigley
//
-// Redistribution and use in source and binary forms, with or without
+// 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,
+// * 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
+// * 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
+// * 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
+//
+// 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"
@@ -101,9 +101,9 @@
}
virtual ~EtherDrive_Node()
- {
- if (ed)
- delete ed;
+ {
+ if (ed)
+ delete ed;
}
bool do_tick()
@@ -141,7 +141,7 @@
int val[6];
ros::Time before = ros::Time::now();
-
+
if (!ed->tick(6,val)) {
return false;
}
@@ -160,7 +160,7 @@
ostringstream oss;
oss << "mot" << i;
- if (frame_id[i] != string("BAD_ID") && parent_id[i] != string("BAD_ID"))
+ if (frame_id[i] != string("BAD_ID") && parent_id[i] != string("BAD_ID"))
{
tf.sendEuler(frame_id[i], parent_id[i],
@@ -187,7 +187,7 @@
usleep(1000);
if (!a.do_tick())
{
- a.log(ros::ERROR,"Etherdrive tick failed.");
+ ROS_ERROR("Etherdrive tick failed.");
break;
}
}
Modified: pkg/trunk/deprecated/etherdrive_old/src/etherdrive_node_new/etherdrive_node.cpp
===================================================================
--- pkg/trunk/deprecated/etherdrive_old/src/etherdrive_node_new/etherdrive_node.cpp 2008-10-22 19:46:56 UTC (rev 5660)
+++ pkg/trunk/deprecated/etherdrive_old/src/etherdrive_node_new/etherdrive_node.cpp 2008-10-22 20:22:26 UTC (rev 5661)
@@ -4,27 +4,27 @@
//
// Copyright (C) 2008, Morgan Quigley
//
-// Redistribution and use in source and binary forms, with or without
+// 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,
+// * 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
+// * 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
+// * 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
+//
+// 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"
@@ -94,38 +94,38 @@
mot_cmd[i].rel = 0;
mot_cmd[i].cmd = 0;
- ed->set_gain(i,'P',0);
+ ed->set_gain(i,'P',0);
ed->set_gain(i,'I',10);
- ed->set_gain(i,'D',0);
- ed->set_gain(i,'W',100);
+ ed->set_gain(i,'D',0);
+ ed->set_gain(i,'W',100);
ed->set_gain(i,'M',1004);
- ed->set_gain(i,'Z',1);
+ ed->set_gain(i,'Z',1);
}
-
+
ed->set_control_mode(1); // 0=voltage, 1=current control
-
-
+
+
firstTimeVelCtrl = true;
fooCounter = 0;
ed->motors_on();
counter = 0;
}
- void mot_callback() {
+ void mot_callback() {
//printf("received motor 3 command %f\n", mot_cmd[3].val);
}
virtual ~EtherDrive_Node()
- {
- if (ed)
- delete ed;
+ {
+ if (ed)
+ delete ed;
}
bool do_tick()
{
-//printf("%i: enc: %i \n", fooCounter, ed->get_enc(5));
+//printf("%i: enc: %i \n", fooCounter, ed->get_enc(5));
int tmp_mot_cmd[6];
int currEncPos[6];
@@ -165,7 +165,7 @@
pos_k_p[2] = -3;
pos_k_p[3] = 1.2;
pos_k_p[4] = 1.2;
- pos_k_p[5] = -1.2; //
+ pos_k_p[5] = -1.2; //
pos_k_i[0] = 0;
pos_k_i[1] = 0;
@@ -179,7 +179,7 @@
pos_k_d[2] = ...
[truncated message content] |
|
From: <rob...@us...> - 2008-10-23 00:41:10
|
Revision: 5689
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5689&view=rev
Author: rob_wheeler
Date: 2008-10-23 00:41:07 +0000 (Thu, 23 Oct 2008)
Log Message:
-----------
Remove unnecessary dependency of ethercat_hardware on mechanism_control_rt
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
pkg/trunk/util/misc_utils/manifest.xml
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-23 00:34:08 UTC (rev 5688)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-10-23 00:41:07 UTC (rev 5689)
@@ -37,7 +37,6 @@
#include <tinyxml/tinyxml.h>
-#include <mechanism_control/mechanism_control.h>
#include <hardware_interface/hardware_interface.h>
#include <al/ethercat_AL.h>
Modified: pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-10-23 00:34:08 UTC (rev 5688)
+++ pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-10-23 00:41:07 UTC (rev 5689)
@@ -9,7 +9,6 @@
<depend package='eml'/>
<depend package='tinyxml'/>
<depend package='roscpp' />
-<depend package='mechanism_control_rt' />
<depend package='loki' />
<depend package='misc_utils' />
<depend package='robot_msgs' />
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-10-23 00:34:08 UTC (rev 5688)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-10-23 00:41:07 UTC (rev 5689)
@@ -37,8 +37,6 @@
#include <ethercat/ethercat_xenomai_drv.h>
#include <dll/ethercat_dll.h>
-#include <ros/node.h>
-
EthercatHardware::EthercatHardware() :
hw_(0), ni_(0), current_buffer_(0), last_buffer_(0), buffer_size_(0), halt_motors_(true), reset_state_(0), publisher_("/diagnostics", 1)
{
@@ -72,8 +70,6 @@
void EthercatHardware::init(char *interface, bool allow_unprogrammed)
{
- ros::node *node = ros::node::instance();
-
// Initialize network interface
interface_ = interface;
if ((ni_ = init_ec(interface)) == NULL)
@@ -180,7 +176,6 @@
void EthercatHardware::initXml(TiXmlElement *config, bool allow_override)
{
- ros::node *node = ros::node::instance();
unsigned int a = 0, s = 0;
for (TiXmlElement *elt = config->FirstChildElement("actuator"); elt; elt = elt->NextSiblingElement("actuator"))
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-23 00:34:08 UTC (rev 5688)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-23 00:41:07 UTC (rev 5689)
@@ -36,7 +36,6 @@
#include <math.h>
-#include <ros/node.h>
#include <ethercat_hardware/wg0x.h>
@@ -189,8 +188,6 @@
int WG0X::initialize(Actuator *actuator, bool allow_unprogrammed)
{
- ros::node *node = ros::node::instance();
-
unsigned int revision = sh_->get_revision();
unsigned int major = (revision >> 8) & 0xff;
unsigned int minor = revision & 0xff;
@@ -270,7 +267,6 @@
void WG0X::initXml(TiXmlElement *elt)
{
- ros::node *node = ros::node::instance();
printf("Overriding actuator: %s\n", actuator_info_.name_);
const char *attr;
Modified: pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
===================================================================
--- pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2008-10-23 00:34:08 UTC (rev 5688)
+++ pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2008-10-23 00:41:07 UTC (rev 5689)
@@ -40,6 +40,7 @@
#include <pthread.h>
#include <string>
+#include <ros/node.h>
#include <rosthread/mutex.h>
#include <rosthread/member_thread.h>
Modified: pkg/trunk/util/misc_utils/manifest.xml
===================================================================
--- pkg/trunk/util/misc_utils/manifest.xml 2008-10-23 00:34:08 UTC (rev 5688)
+++ pkg/trunk/util/misc_utils/manifest.xml 2008-10-23 00:41:07 UTC (rev 5689)
@@ -8,6 +8,7 @@
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
<depend package="rosthread" />
+ <depend package="roscpp" />
<export>
<cpp cflags="-I${prefix}/include" />
</export>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-23 18:14:04
|
Revision: 5716
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5716&view=rev
Author: gerkey
Date: 2008-10-23 18:13:59 +0000 (Thu, 23 Oct 2008)
Log Message:
-----------
Removed pil from 3rdparty; added sysdepends on python-imaging where appropriate
Modified Paths:
--------------
pkg/trunk/util/pyrob/manifest.xml
pkg/trunk/vision/star_detector/manifest.xml
pkg/trunk/world_models/static_map_server/manifest.xml
Removed Paths:
-------------
pkg/trunk/3rdparty/pil/
Modified: pkg/trunk/util/pyrob/manifest.xml
===================================================================
--- pkg/trunk/util/pyrob/manifest.xml 2008-10-23 17:42:14 UTC (rev 5715)
+++ pkg/trunk/util/pyrob/manifest.xml 2008-10-23 18:13:59 UTC (rev 5716)
@@ -6,6 +6,7 @@
<author>Hai Nguyen, Advait Jain, Charlie Kemp, Cressel Anderson</author>
<license>BSD</license>
<depend package="numpy" />
- <depend package="pil" />
+ <sysdepend os="ubuntu" version="7.04-feisty" package="python-imaging"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="python-imaging"/>
</package>
Modified: pkg/trunk/vision/star_detector/manifest.xml
===================================================================
--- pkg/trunk/vision/star_detector/manifest.xml 2008-10-23 17:42:14 UTC (rev 5715)
+++ pkg/trunk/vision/star_detector/manifest.xml 2008-10-23 18:13:59 UTC (rev 5716)
@@ -9,7 +9,6 @@
<depend package="rospy"/>
<depend package="boost"/>
<depend package="opencv_latest"/>
- <depend package="pil"/>
<export>
<cpp cflags="-I${prefix}/include" lflags=" -L${prefix}/lib -lstarfeature"/>
</export>
Modified: pkg/trunk/world_models/static_map_server/manifest.xml
===================================================================
--- pkg/trunk/world_models/static_map_server/manifest.xml 2008-10-23 17:42:14 UTC (rev 5715)
+++ pkg/trunk/world_models/static_map_server/manifest.xml 2008-10-23 18:13:59 UTC (rev 5716)
@@ -9,6 +9,7 @@
<depend package="std_msgs" />
<depend package="std_srvs" />
<depend package="rostest" />
- <depend package="pil" />
<depend package="pyyaml" />
+ <sysdepend os="ubuntu" version="7.04-feisty" package="python-imaging"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="python-imaging"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-10-24 22:15:40
|
Revision: 5757
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5757&view=rev
Author: rob_wheeler
Date: 2008-10-24 22:15:18 +0000 (Fri, 24 Oct 2008)
Log Message:
-----------
Move SetJointCmd, GetJointCmd, and JointCmd to robot_srvs and robot_msgs
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
pkg/trunk/nav/teleop_base/manifest.xml
pkg/trunk/nav/teleop_base/teleop_head.cpp
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/JointCmd.msg
pkg/trunk/robot_srvs/srv/GetJointCmd.srv
pkg/trunk/robot_srvs/srv/SetJointCmd.srv
Removed Paths:
-------------
pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv
pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h 2008-10-24 22:15:18 UTC (rev 5757)
@@ -44,8 +44,8 @@
#include <robot_mechanism_controllers/joint_effort_controller.h>
// Services
-#include <pr2_mechanism_controllers/SetJointCmd.h>
-#include <pr2_mechanism_controllers/GetJointCmd.h>
+#include <robot_srvs/SetJointCmd.h>
+#include <robot_srvs/GetJointCmd.h>
#include <pr2_mechanism_controllers/SetJointGains.h>
#include <pr2_mechanism_controllers/GetJointGains.h>
@@ -99,9 +99,9 @@
// * @brief Overloaded method for convenience
// * @param req
// */
-// void setJointCmd(pr2_mechanism_controllers::SetJointPosCmd::request &req);
+// void setJointCmd(robot_msgs::SetJointPosCmd::request &req);
- void getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const;
+ void getJointCmd(robot_msgs::JointCmd & cmd) const;
/*!
* \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
@@ -216,8 +216,8 @@
* @param resp The response is empty
* @return
*/
- bool setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
- pr2_mechanism_controllers::SetJointCmd::response &resp);
+ bool setJointSrv(robot_srvs::SetJointCmd::request &req,
+ robot_srvs::SetJointCmd::response &resp);
/** @brief service that returns the goal of the controller
* @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
@@ -225,11 +225,11 @@
* @param resp the response, contains a JointPosCmd message with the goal of the controller
* @return
*/
- bool getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
- pr2_mechanism_controllers::GetJointCmd::response &resp);
+ bool getJointCmd(robot_srvs::GetJointCmd::request &req,
+ robot_srvs::GetJointCmd::response &resp);
private:
- pr2_mechanism_controllers::JointCmd msg_; //The message used by the ROS callback
+ robot_msgs::JointCmd msg_; //The message used by the ROS callback
ArmDynamicsController *c_;
};
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h 2008-10-24 22:15:18 UTC (rev 5757)
@@ -46,8 +46,8 @@
#include <robot_mechanism_controllers/joint_position_controller.h>
// Services
-#include <pr2_mechanism_controllers/SetJointCmd.h>
-#include <pr2_mechanism_controllers/GetJointCmd.h>
+#include <robot_srvs/SetJointCmd.h>
+#include <robot_srvs/GetJointCmd.h>
#include <std_msgs/PointStamped.h>
// Math utils
@@ -119,7 +119,7 @@
*
* \param cmd names and positions.
*/
- void getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const;
+ void getJointCmd(robot_msgs::JointCmd & cmd) const;
/*!
* \brief Returns the position of the controller in the vector.
@@ -185,8 +185,8 @@
* \param req (names, positions)
* \param resp (names, positions)
*/
- bool setJointCmd(pr2_mechanism_controllers::SetJointCmd::request &req,
- pr2_mechanism_controllers::SetJointCmd::response &resp);
+ bool setJointCmd(robot_srvs::SetJointCmd::request &req,
+ robot_srvs::SetJointCmd::response &resp);
/*!
* \brief Gets the commands for all the joints managed by the controller at once.
@@ -194,8 +194,8 @@
* \param req
* \param resp (positions)
*/
- bool getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
- pr2_mechanism_controllers::GetJointCmd::response &resp);
+ bool getJointCmd(robot_srvs::GetJointCmd::request &req,
+ robot_srvs::GetJointCmd::response &resp);
/*!
* \brief Tracks a point in a specified frame.
*
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2008-10-24 22:15:18 UTC (rev 5757)
@@ -16,6 +16,7 @@
<depend package="std_msgs" />
<depend package="math_utils" />
<depend package="robot_kinematics" />
+ <depend package="xenomai" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg 2008-10-24 22:15:18 UTC (rev 5757)
@@ -1,4 +0,0 @@
-string[] names
-float64[] positions
-float64[] velocity
-float64[] acc
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp 2008-10-24 22:15:18 UTC (rev 5757)
@@ -126,7 +126,7 @@
}
-void ArmDynamicsController::getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const
+void ArmDynamicsController::getJointCmd(robot_msgs::JointCmd & cmd) const
{
const unsigned int n = joint_effort_controllers_.size();
cmd.set_names_size(n);
@@ -317,8 +317,8 @@
return false;
}
-bool ArmDynamicsControllerNode::setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
- pr2_mechanism_controllers::SetJointCmd::response &resp)
+bool ArmDynamicsControllerNode::setJointSrv(robot_srvs::SetJointCmd::request &req,
+ robot_srvs::SetJointCmd::response &resp)
{
std::vector<double> pos;
std::vector<double> vel;
@@ -334,10 +334,10 @@
return true;
}
-bool ArmDynamicsControllerNode::getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
- pr2_mechanism_controllers::GetJointCmd::response &resp)
+bool ArmDynamicsControllerNode::getJointCmd(robot_srvs::GetJointCmd::request &req,
+ robot_srvs::GetJointCmd::response &resp)
{
- pr2_mechanism_controllers::JointCmd cmd;
+ robot_msgs::JointCmd cmd;
c_->getJointCmd(cmd);
resp.command = cmd;
return true;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2008-10-24 22:15:18 UTC (rev 5757)
@@ -97,7 +97,7 @@
}
-void HeadPanTiltController::getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const
+void HeadPanTiltController::getJointCmd(robot_msgs::JointCmd & cmd) const
{
const unsigned int n = joint_position_controllers_.size();
cmd.set_names_size(n);
@@ -201,12 +201,12 @@
return false;
}
-bool HeadPanTiltControllerNode::setJointCmd(pr2_mechanism_controllers::SetJointCmd::request &req,
- pr2_mechanism_controllers::SetJointCmd::response &resp)
+bool HeadPanTiltControllerNode::setJointCmd(robot_srvs::SetJointCmd::request &req,
+ robot_srvs::SetJointCmd::response &resp)
{
std::vector<double> pos;
std::vector<std::string> names;
- pr2_mechanism_controllers::JointCmd cmds;
+ robot_msgs::JointCmd cmds;
req.get_positions_vec(pos);
req.get_names_vec(names);
@@ -218,10 +218,10 @@
return true;
}
-bool HeadPanTiltControllerNode::getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
- pr2_mechanism_controllers::GetJointCmd::response &resp)
+bool HeadPanTiltControllerNode::getJointCmd(robot_srvs::GetJointCmd::request &req,
+ robot_srvs::GetJointCmd::response &resp)
{
- pr2_mechanism_controllers::JointCmd cmd;
+ robot_msgs::JointCmd cmd;
c_->getJointCmd(cmd);
resp.command = cmd;
return true;
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv 2008-10-24 22:15:18 UTC (rev 5757)
@@ -1,2 +0,0 @@
----
-JointCmd command
\ No newline at end of file
Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv 2008-10-24 22:15:18 UTC (rev 5757)
@@ -1,9 +0,0 @@
-float64[] positions
-float64[] velocity
-float64[] acc
-string[] names
----
-float64[] positions
-float64[] velocity
-float64[] acc
-string[] names
\ No newline at end of file
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-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp 2008-10-24 22:15:18 UTC (rev 5757)
@@ -34,7 +34,7 @@
#include <libTF/libTF.h>
#include <ros/node.h>
-#include <pr2_mechanism_controllers/SetJointCmd.h>
+#include <robot_srvs/SetJointCmd.h>
#include <std_msgs/TransformWithRateStamped.h>
#include <std_msgs/BaseVel.h>
#include <std_msgs/RobotBase2DOdom.h>
@@ -75,8 +75,8 @@
signal(SIGTERM, finalize);
/*********** Start moving the arm ************/
- pr2_mechanism_controllers::SetJointCmd::request req;
- pr2_mechanism_controllers::SetJointCmd::response resp;
+ robot_srvs::SetJointCmd::request req;
+ robot_srvs::SetJointCmd::response resp;
int num_joints = 7;
Modified: pkg/trunk/nav/teleop_base/manifest.xml
===================================================================
--- pkg/trunk/nav/teleop_base/manifest.xml 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/nav/teleop_base/manifest.xml 2008-10-24 22:15:18 UTC (rev 5757)
@@ -3,5 +3,5 @@
<license>BSD</license>
<depend package="joy"/>
<depend package="std_msgs"/>
- <depend package="pr2_mechanism_controllers"/>
+ <depend package="robot_srvs"/>
</package>
Modified: pkg/trunk/nav/teleop_base/teleop_head.cpp
===================================================================
--- pkg/trunk/nav/teleop_base/teleop_head.cpp 2008-10-24 22:05:43 UTC (rev 5756)
+++ pkg/trunk/nav/teleop_base/teleop_head.cpp 2008-10-24 22:15:18 UTC (rev 5757)
@@ -3,7 +3,7 @@
#include "ros/node.h"
#include "joy/Joy.h"
-#include "pr2_mechanism_controllers/SetJointCmd.h"
+#include "robot_srvs/SetJointCmd.h"
using namespace ros;
@@ -71,8 +71,8 @@
req_tilt = std::max(std::min(req_tilt, max_tilt), -max_tilt);
}
- pr2_mechanism_controllers::SetJointCmd::request req;
- pr2_mechanism_controllers::SetJointCmd::response res;
+ robot_srvs::SetJointCmd::request req;
+ robot_srvs::SetJointCmd::response res;
req.positions.push_back(req_pan);
req.positions.push_back(req_tilt);
req.velocity.push_back(0.0);
Copied: pkg/trunk/robot_msgs/msg/JointCmd.msg (from rev 5691, pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg)
===================================================================
--- pkg/trunk/robot_msgs/msg/JointCmd.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/JointCmd.msg 2008-10-24 22:15:18 UTC (rev 5757)
@@ -0,0 +1,4 @@
+string[] names
+float64[] positions
+float64[] velocity
+float64[] acc
Copied: pkg/trunk/robot_srvs/srv/GetJointCmd.srv (from rev 5691, pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv)
===================================================================
--- pkg/trunk/robot_srvs/srv/GetJointCmd.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/GetJointCmd.srv 2008-10-24 22:15:18 UTC (rev 5757)
@@ -0,0 +1,2 @@
+---
+robot_msgs/JointCmd command
Copied: pkg/trunk/robot_srvs/srv/SetJointCmd.srv (from rev 5691, pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv)
===================================================================
--- pkg/trunk/robot_srvs/srv/SetJointCmd.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/SetJointCmd.srv 2008-10-24 22:15:18 UTC (rev 5757)
@@ -0,0 +1,9 @@
+float64[] positions
+float64[] velocity
+float64[] acc
+string[] names
+---
+float64[] positions
+float64[] velocity
+float64[] acc
+string[] names
\ 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: <tf...@us...> - 2008-10-24 23:57:44
|
Revision: 5771
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5771&view=rev
Author: tfoote
Date: 2008-10-24 23:57:40 +0000 (Fri, 24 Oct 2008)
Log Message:
-----------
executing PointCloudFloat32 to PointCloud and Point3DFloat32 to Point32
Modified Paths:
--------------
pkg/trunk/deprecated/exec_trex/ROSNode.cc
pkg/trunk/deprecated/exec_trex/test/test.wpc.0.py
pkg/trunk/deprecated/exec_trex/test/test.wpc.1.py
pkg/trunk/deprecated/overhead_grasp_behavior/src/overhead_grasp.cc
pkg/trunk/deprecated/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/deprecated/rosControllers/include/rosControllers/RosJoint.h
pkg/trunk/deprecated/rosControllers/include/rosControllers/RosJointController.h
pkg/trunk/drivers/cam/dc1394_cam_server/src/dc1394_cam_server/dc1394_cam_server.cpp
pkg/trunk/drivers/imu/imu_node/msg/ImuData.msg
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/BuildCloud.srv
pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
pkg/trunk/drivers/robot/sensor_cart/src/sensor_recorder.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
pkg/trunk/grasp_module/grasp_learner/src/scan_voxelizer.cpp
pkg/trunk/grasp_module/grasp_module/msg/object_msg.msg
pkg/trunk/grasp_module/grasp_module/srv/object_detector_srv.srv
pkg/trunk/grasp_module/object_detector/src/objectDetector.cpp
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/test.wpc.0.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/test.wpc.1.py
pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/manip/arm_trajectory/arm_trajectory.cc
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
pkg/trunk/nav/sbpl_2dnav/src/sbpl_2dnav.cpp
pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg
pkg/trunk/simulators/simulator_integration_tests/world_3d_map_gazebo_tests/testcloud.py
pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h
pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
pkg/trunk/util/rosTF/include/rosTF/rosTF.h
pkg/trunk/util/rosTF/src/rosTF.cpp
pkg/trunk/util/tf/mainpage.dox
pkg/trunk/vision/calib_converter/src/calib_converter.cpp
pkg/trunk/vision/dorylus/include/dorylus_node.h
pkg/trunk/vision/dorylus/src/dorylus_node.cpp
pkg/trunk/vision/scan_utils/include/dataTypes.h
pkg/trunk/vision/scan_utils/include/listen_node/scanListenNode.h
pkg/trunk/vision/scan_utils/include/smartScan.h
pkg/trunk/vision/scan_utils/msg/OctreeMsg.msg
pkg/trunk/vision/scan_utils/src/cloudToOctree/cloudToOctree.cpp
pkg/trunk/vision/scan_utils/src/dataTypes.cpp
pkg/trunk/vision/scan_utils/src/listen_node/scanListenNode.cpp
pkg/trunk/vision/scan_utils/src/smartScan.cpp
pkg/trunk/vision/spacetime_stereo/sts_dynamic.cpp
pkg/trunk/vision/spacetime_stereo/sts_node.cpp
pkg/trunk/visualization/cloud_viewer/src/cloud_node/cloud_node.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h
pkg/trunk/visualization/scene_labeler/include/scene_labeler.h
pkg/trunk/visualization/scene_labeler/include/scene_labeler_stereo.h
pkg/trunk/visualization/scene_labeler/src/scene_labeler/scene_labeler.cpp
pkg/trunk/visualization/scene_labeler/src/scene_labeler/scene_labeler_stereo.cpp
pkg/trunk/visualization/scene_labeler/src/smallv_transformer/smallv_transformer.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/costmap_2d_ros.cpp
pkg/trunk/world_models/costmap_2d/src/test/benchmark.cc
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/deprecated/exec_trex/ROSNode.cc
===================================================================
--- pkg/trunk/deprecated/exec_trex/ROSNode.cc 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/deprecated/exec_trex/ROSNode.cc 2008-10-24 23:57:40 UTC (rev 5771)
@@ -872,11 +872,11 @@
// Assemble a point cloud, in the laser's frame
- std_msgs::PointCloudFloat32 local_cloud;
+ std_msgs::PointCloud local_cloud;
projector_.projectLaser(*it, local_cloud, laser_maxrange);
// Convert to a point cloud in the map frame
- std_msgs::PointCloudFloat32 global_cloud;
+ std_msgs::PointCloud global_cloud;
try
{
Modified: pkg/trunk/deprecated/exec_trex/test/test.wpc.0.py
===================================================================
--- pkg/trunk/deprecated/exec_trex/test/test.wpc.0.py 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/deprecated/exec_trex/test/test.wpc.0.py 2008-10-24 23:57:40 UTC (rev 5771)
@@ -68,7 +68,7 @@
self.success = True
def test_wpc(self):
- rospy.TopicSub("groundtruthposition", Point3DFloat32, self.positionInput)
+ rospy.TopicSub("groundtruthposition", Point32, self.positionInput)
self.test.advertiseInitalPose2d(NAME)
rospy.ready(NAME, anonymous=True)
time.sleep(2.0)
Modified: pkg/trunk/deprecated/exec_trex/test/test.wpc.1.py
===================================================================
--- pkg/trunk/deprecated/exec_trex/test/test.wpc.1.py 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/deprecated/exec_trex/test/test.wpc.1.py 2008-10-24 23:57:40 UTC (rev 5771)
@@ -68,7 +68,7 @@
self.success = True
def test_wpc(self):
- rospy.TopicSub("groundtruthposition", Point3DFloat32, self.positionInput)
+ rospy.TopicSub("groundtruthposition", Point32, self.positionInput)
self.test.advertiseInitalPose2d(NAME)
rospy.ready(NAME, anonymous=True)
time.sleep(2.0)
Modified: pkg/trunk/deprecated/overhead_grasp_behavior/src/overhead_grasp.cc
===================================================================
--- pkg/trunk/deprecated/overhead_grasp_behavior/src/overhead_grasp.cc 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/deprecated/overhead_grasp_behavior/src/overhead_grasp.cc 2008-10-24 23:57:40 UTC (rev 5771)
@@ -48,7 +48,7 @@
@section topic ROS topics
Subscribes to (name/type):
- - @b "object_position" / @b std_msgs::Point3DFloat32 : 3d Pose of the object. This is the cheat laser pointer interface.
+ - @b "object_position" / @b std_msgs::Point32 : 3d Pose of the object. This is the cheat laser pointer interface.
- @b "rightarm_tooltip_cartesian" / @b pr2_msgs::EndEffectorState : current cartesian pose of the end effector.
Publishes to (name/type):
@@ -91,7 +91,7 @@
#include <pr2_kinematic_controllers/Float64Int32.h>
-#include <std_msgs/Point3DFloat32.h>
+#include <std_msgs/Point32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/PR2Arm.h>
#include <unistd.h>
@@ -109,7 +109,7 @@
public:
// coordinates of arm's origin in gazebo/arm coordinate frame (the axes of both frames are parallel)
Vector gazebo_to_arm_vector;
- std_msgs::Point3DFloat32 objectPosMsg;
+ std_msgs::Point32 objectPosMsg;
pr2_msgs::EndEffectorState rightEndEffectorMsg;
Frame right_tooltip_frame;
Vector objectPosition;
Modified: pkg/trunk/deprecated/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/deprecated/planning_world_viewer/src/planning_world_viewer.cpp 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/deprecated/planning_world_viewer/src/planning_world_viewer.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -65,7 +65,7 @@
Additional subscriptions due to inheritance from NodeCollisionModel:
- @b localizedpose/RobotBase2DOdom : localized position of the robot base
-- @b world_3d_map/PointCloudFloat32 : point cloud with data describing the 3D environment
+- @b world_3d_map/PointCloud : point cloud with data describing the 3D environment
Publishes to (name/type):
- None
Modified: pkg/trunk/deprecated/rosControllers/include/rosControllers/RosJoint.h
===================================================================
--- pkg/trunk/deprecated/rosControllers/include/rosControllers/RosJoint.h 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/deprecated/rosControllers/include/rosControllers/RosJoint.h 2008-10-24 23:57:40 UTC (rev 5771)
@@ -31,8 +31,8 @@
// roscpp - laser
#include <std_msgs/LaserScan.h>
// roscpp - laser image (point cloud)
-#include <std_msgs/PointCloudFloat32.h>
-#include <std_msgs/Point3DFloat32.h>
+#include <std_msgs/PointCloud.h>
+#include <std_msgs/Point32.h>
#include <std_msgs/ChannelFloat32.h>
// roscpp - used for shutter message right now
#include <std_msgs/Empty.h>
Modified: pkg/trunk/deprecated/rosControllers/include/rosControllers/RosJointController.h
===================================================================
--- pkg/trunk/deprecated/rosControllers/include/rosControllers/RosJointController.h 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/deprecated/rosControllers/include/rosControllers/RosJointController.h 2008-10-24 23:57:40 UTC (rev 5771)
@@ -32,8 +32,8 @@
// roscpp - laser
#include <std_msgs/LaserScan.h>
// roscpp - laser image (point cloud)
-#include <std_msgs/PointCloudFloat32.h>
-#include <std_msgs/Point3DFloat32.h>
+#include <std_msgs/PointCloud.h>
+#include <std_msgs/Point32.h>
#include <std_msgs/ChannelFloat32.h>
// roscpp - used for shutter message right now
#include <std_msgs/Empty.h>
Modified: pkg/trunk/drivers/cam/dc1394_cam_server/src/dc1394_cam_server/dc1394_cam_server.cpp
===================================================================
--- pkg/trunk/drivers/cam/dc1394_cam_server/src/dc1394_cam_server/dc1394_cam_server.cpp 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/cam/dc1394_cam_server/src/dc1394_cam_server/dc1394_cam_server.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -37,7 +37,7 @@
#include "ros/node.h"
#include "std_msgs/Image.h"
#include "std_msgs/ImageArray.h"
-#include "std_msgs/PointCloudFloat32.h"
+#include "std_msgs/PointCloud.h"
#include "std_msgs/Empty.h"
#include "std_msgs/String.h"
@@ -78,7 +78,7 @@
public:
std_msgs::ImageArray img_;
- std_msgs::PointCloudFloat32 cloud_;
+ std_msgs::PointCloud cloud_;
ros::Time next_time_;
@@ -386,7 +386,7 @@
{
advertise<std_msgs::String>(cd.name + string("/cal_params"), 1);
advertise<std_msgs::ImageArray>(cd.name + string("/images"), 1);
- advertise<std_msgs::PointCloudFloat32>(cd.name + string("/cloud"), 1);
+ advertise<std_msgs::PointCloud>(cd.name + string("/cloud"), 1);
} else {
advertise<std_msgs::Image>(cd.name + string("/image"), 1);
advertise<std_msgs::Image>(cd.name + string("/images"), 1);
Modified: pkg/trunk/drivers/imu/imu_node/msg/ImuData.msg
===================================================================
--- pkg/trunk/drivers/imu/imu_node/msg/ImuData.msg 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/imu/imu_node/msg/ImuData.msg 2008-10-24 23:57:40 UTC (rev 5771)
@@ -1,3 +1,3 @@
Header header
-std_msgs/Point3DFloat32 accel
-std_msgs/Point3DFloat32 angrate
+std_msgs/Point32 accel
+std_msgs/Point32 angrate
Modified: 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 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -40,7 +40,7 @@
#include "point_cloud_assembler/BuildCloud.h"
// Messages
-#include "std_msgs/PointCloudFloat32.h"
+#include "std_msgs/PointCloud.h"
using namespace std_msgs ;
@@ -59,7 +59,7 @@
GrabCloudData() : ros::node("grab_cloud_data")
{
- advertise<PointCloudFloat32> ("full_cloud", 1) ;
+ advertise<PointCloud> ("full_cloud", 1) ;
}
~GrabCloudData()
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-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -66,7 +66,7 @@
#include "ros/node.h"
#include "rosTF/rosTF.h"
#include "std_msgs/LaserScan.h"
-#include "std_msgs/PointCloudFloat32.h"
+#include "std_msgs/PointCloud.h"
#include <deque>
@@ -141,8 +141,8 @@
unsigned int cloud_count = 0 ; // Store the number of points in the current cloud
- 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
+ PointCloud projector_cloud ; // Stores the current scan after being projected into the laser frame
+ PointCloud target_frame_cloud ; // Stores the current scan in the target frame
unsigned int i = 0 ;
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -38,7 +38,7 @@
#include "point_cloud_assembler/BuildCloud.h"
// Messages
-#include "std_msgs/PointCloudFloat32.h"
+#include "std_msgs/PointCloud.h"
#include "pr2_mechanism_controllers/LaserScannerSignal.h"
using namespace std_msgs ;
@@ -62,7 +62,7 @@
{
prev_signal_.header.stamp.fromNSec(0) ;
- advertise<PointCloudFloat32> ("full_cloud", 1) ;
+ advertise<PointCloud> ("full_cloud", 1) ;
subscribe("laser_scanner_signal", cur_signal_, &PointCloudSnapshotter::scannerSignalCallback, 40) ;
}
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/BuildCloud.srv
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/BuildCloud.srv 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/BuildCloud.srv 2008-10-24 23:57:40 UTC (rev 5771)
@@ -2,4 +2,4 @@
time end
string target_frame_id
---
-std_msgs/PointCloudFloat32 cloud
\ No newline at end of file
+std_msgs/PointCloud cloud
\ No newline at end of file
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -78,9 +78,9 @@
- @b "mot"/<a href="../../std_msgs/html/classstd__msgs_1_1Actuator.html">Actuator</a> : encoder data from the tilting stage
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 "cloud"/<a href="../../std_msgs/html/classstd__msgs_1_1PointCloud.html">PointCloud</a> : Incremental cloud data. Each scan from the laser is converted into a PointCloud.
- @b "shutter"/<a href="../../std_msgs/html/classstd__msgs_1_1Empty.html">Empty</a> : An empty message is sent to indicate a full sweep has occured
-- @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
+- @b "full_cloud"/<a href="../../std_msgs/html/classstd__msgs_1_1PointCloud.html">PointCloud</a> : A full point cloud containing all of the points between the last two shutters
- @b "laser_image"/<a href="../../std_msgs/html/classstd__msgs_1_1LaserImage.html">LaserImage</a> : A representation of the full point cloud as a pair of pseudo-images
- @b "image"/<a href="../../std_msgs/html/classstd__msgs_1_1Image.html">Image</a> : The intensity image from the LaserImage
- @b "mot_cmd"/<a href="../../std_msgs/html/classstd__msgs_1_1Actuator.html">Actuator</a> : The commanded position of the tilting stage
@@ -102,7 +102,7 @@
#include "rosTF/rosTF.h"
#include "std_msgs/Empty.h"
#include "std_msgs/LaserScan.h"
-#include "std_msgs/PointCloudFloat32.h"
+#include "std_msgs/PointCloud.h"
#include "std_msgs/LaserImage.h"
#include "std_msgs/Actuator.h"
@@ -120,7 +120,7 @@
rosTFClient tf;
laser_scan::LaserProjection projector;
- PointCloudFloat32 cloud;
+ PointCloud cloud;
Empty shutter;
Actuator cmd;
@@ -128,7 +128,7 @@
Actuator encoder;
LaserImage image;
- PointCloudFloat32 full_cloud;
+ PointCloud full_cloud;
int full_cloud_cnt;
int num_scans;
@@ -155,9 +155,9 @@
Tilting_Laser() : ros::node("tilting_laser"), tf(*this), full_cloud_cnt(0), sizes_ready(false), img_ind(-1), img_dir(1),last_ang(1000000), rate_err_down(0.0), rate_err_up(0.0), accum_angle(0.0), scan_received(false), count(0)
{
- advertise<PointCloudFloat32>("cloud", 1);
+ advertise<PointCloud>("cloud", 1);
advertise<Empty>("shutter", 1);
- advertise<PointCloudFloat32>("full_cloud", 1);
+ advertise<PointCloud>("full_cloud", 1);
advertise<LaserImage>("laser_image", 1);
advertise<Image>("image", 1);
advertise<Actuator>("mot_cmd", 100);
@@ -276,7 +276,7 @@
int cnt = 0;
- std_msgs::PointCloudFloat32 temp_cloud;
+ std_msgs::PointCloud temp_cloud;
projector.projectLaser(scans, temp_cloud);
cloud = tf.transformPointCloud("FRAMEID_TILT_BASE", temp_cloud);
Modified: pkg/trunk/drivers/robot/sensor_cart/src/sensor_recorder.cpp
===================================================================
--- pkg/trunk/drivers/robot/sensor_cart/src/sensor_recorder.cpp 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/robot/sensor_cart/src/sensor_recorder.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -5,7 +5,7 @@
#include "opencv/highgui.h"
#include "ros/node.h"
#include "std_msgs/LaserImage.h"
-#include "std_msgs/PointCloudFloat32.h"
+#include "std_msgs/PointCloud.h"
#include "image_utils/cv_bridge.h"
#include <sys/stat.h>
@@ -19,11 +19,11 @@
public:
LaserImage laser_img;
Image cam_img;
- PointCloudFloat32 cloud;
+ PointCloud cloud;
LaserImage laser_img_sv;
Image cam_img_sv;
- PointCloudFloat32 cloud_sv;
+ PointCloud cloud_sv;
CvBridge<Image> cam_bridge;
CvBridge<Image> laser_int_bridge;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-10-24 23:57:40 UTC (rev 5771)
@@ -292,7 +292,7 @@
<tr><td> \b base_scan </td> <td> std_msgs::LaserScan.msg </td> <td> Ros_Laser.hh </td> <td> Laser scans from Hokuyo at the base. </td> </tr>
<tr><td> \b tilt_scan </td> <td> std_msgs::LaserScan.msg </td> <td> Ros_Laser.hh </td> <td> Laser scans from tilting Hokuyo. </td> </tr>
- <tr><td> \b full_cloud </td> <td> std_msgs::PointCloudFloat32.msg </td> <td> Ros_Block_Laser.hh </td> <td> Simulated point clouds data from stereo camera. </td> </tr>
+ <tr><td> \b full_cloud </td> <td> std_msgs::PointCloud.msg </td> <td> Ros_Block_Laser.hh </td> <td> Simulated point clouds data from stereo camera. </td> </tr>
<tr><td> \b battery_state </td> <td> robot_msgs::BatteryStzte </td> <td> gazebo_battery.h </td> <td> Simulated battery state. </td> </tr>
<tr><td> \b diagnostic </td> <td> robot_msgs::DiagnosticMessage </td> <td> gazebo_battery.h </td> <td> Simulated battery diagnostic messages. </td> </tr>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-10-24 23:57:40 UTC (rev 5771)
@@ -32,7 +32,7 @@
#include <ros/node.h>
#include <std_msgs/LaserScan.h>
-#include <std_msgs/PointCloudFloat32.h>
+#include <std_msgs/PointCloud.h>
namespace gazebo
{
@@ -90,7 +90,7 @@
\brief ROS laser block simulation.
\li Starts a ROS node if none exists.
\li This controller simulates a block of laser range detections.
- Resulting point cloud (std_msgs::PointCloudFloat32.msg) is published as a ROS topic.
+ Resulting point cloud (std_msgs::PointCloud.msg) is published as a ROS topic.
\li Example Usage:
\verbatim
<model:physical name="ray_model">
@@ -162,7 +162,7 @@
private: ros::node *rosnode;
/// \brief ros message
- private: std_msgs::PointCloudFloat32 cloudMsg;
+ private: std_msgs::PointCloud cloudMsg;
/// \brief topic name
private: std::string topicName;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc 2008-10-24 23:57:40 UTC (rev 5771)
@@ -81,7 +81,7 @@
{
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
std::cout << "================= " << this->topicName << std::endl;
- rosnode->advertise<std_msgs::PointCloudFloat32>(this->topicName,10);
+ rosnode->advertise<std_msgs::PointCloud>(this->topicName,10);
this->frameName = node->GetString("frameName","default_ros_laser",0); //read from xml file
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
Modified: pkg/trunk/grasp_module/grasp_learner/src/scan_voxelizer.cpp
===================================================================
--- pkg/trunk/grasp_module/grasp_learner/src/scan_voxelizer.cpp 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/grasp_module/grasp_learner/src/scan_voxelizer.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -6,7 +6,7 @@
#include "grasp_learner_types.h"
#include "grasp_learner/OctreeLearningMsg.h"
-#include "std_msgs/Point3DFloat32.h"
+#include "std_msgs/Point32.h"
#include <fstream>
#include <vector>
Modified: pkg/trunk/grasp_module/grasp_module/msg/object_msg.msg
===================================================================
--- pkg/trunk/grasp_module/grasp_module/msg/object_msg.msg 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/grasp_module/grasp_module/msg/object_msg.msg 2008-10-24 23:57:40 UTC (rev 5771)
@@ -1,4 +1,4 @@
-std_msgs/Point3DFloat32 centroid
-std_msgs/Point3DFloat32 axis1
-std_msgs/Point3DFloat32 axis2
-std_msgs/Point3DFloat32 axis3
\ No newline at end of file
+std_msgs/Point32 centroid
+std_msgs/Point32 axis1
+std_msgs/Point32 axis2
+std_msgs/Point32 axis3
\ No newline at end of file
Modified: pkg/trunk/grasp_module/grasp_module/srv/object_detector_srv.srv
===================================================================
--- pkg/trunk/grasp_module/grasp_module/srv/object_detector_srv.srv 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/grasp_module/grasp_module/srv/object_detector_srv.srv 2008-10-24 23:57:40 UTC (rev 5771)
@@ -1,4 +1,4 @@
-std_msgs/PointCloudFloat32 cloud
+std_msgs/PointCloud cloud
float32 connectedThreshold
int32 minComponentSize
float32 grazingThreshold
Modified: pkg/trunk/grasp_module/object_detector/src/objectDetector.cpp
===================================================================
--- pkg/trunk/grasp_module/object_detector/src/objectDetector.cpp 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/grasp_module/object_detector/src/objectDetector.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -95,7 +95,7 @@
if (mRemThresh != 0) {
//find table
fprintf(stderr,"Finding table...");
- std_msgs::Point3DFloat32 planePoint, planeNormal;
+ std_msgs::Point32 planePoint, planeNormal;
float fitValue = scan->ransacPlane(planePoint, planeNormal);
fprintf(stderr," done.\n");
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -1,6 +1,6 @@
#include "ros/node.h"
-#include "std_msgs/Point3DFloat32.h"
+#include "std_msgs/Point32.h"
#include <rosTF/rosTF.h>
using std::string;
@@ -10,11 +10,11 @@
*/
class GroundTruthTransform : public ros::node {
public:
- std_msgs::Point3DFloat32 msg;
+ std_msgs::Point32 msg;
rosTFClient tf;
GroundTruthTransform() : ros::node("GroundTruthTransform"), tf(*this, false) {
- advertise<std_msgs::Point3DFloat32>("groundtruthposition");
+ advertise<std_msgs::Point32>("groundtruthposition");
}
void speak() {
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/test.wpc.0.py
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/test.wpc.0.py 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/test.wpc.0.py 2008-10-24 23:57:40 UTC (rev 5771)
@@ -68,7 +68,7 @@
self.success = True
def test_wpc(self):
- rospy.TopicSub("groundtruthposition", Point3DFloat32, self.positionInput)
+ rospy.TopicSub("groundtruthposition", Point32, self.positionInput)
self.test.advertiseInitalPose2d(NAME)
rospy.ready(NAME, anonymous=True)
time.sleep(2.0)
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/test.wpc.1.py
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/test.wpc.1.py 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/test.wpc.1.py 2008-10-24 23:57:40 UTC (rev 5771)
@@ -67,7 +67,7 @@
self.success = True
def test_wpc(self):
- rospy.TopicSub("groundtruthposition", Point3DFloat32, self.positionInput)
+ rospy.TopicSub("groundtruthposition", Point32, self.positionInput)
self.test.advertiseInitalPose2d(NAME)
rospy.ready(NAME, anonymous=True)
time.sleep(2.0)
Modified: pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/highlevel/highlevel_controllers/include/MoveBase.hh 2008-10-24 23:57:40 UTC (rev 5771)
@@ -144,7 +144,7 @@
/**
* @brief Helper method to update the costmap and conduct other book-keeping
*/
- void updateDynamicObstacles(double ts, const std_msgs::PointCloudFloat32& cloud);
+ void updateDynamicObstacles(double ts, const std_msgs::PointCloud& cloud);
/**
* @brief Issue zero velocity commands
@@ -175,7 +175,7 @@
std_msgs::LaserScan laserScanMsg_; /**< Filled by subscriber with new laser scans */
- std_msgs::PointCloudFloat32 pointCloudMsg_; /**< Filled by subscriber with point clouds */
+ std_msgs::PointCloud pointCloudMsg_; /**< Filled by subscriber with point clouds */
std_msgs::RobotBase2DOdom odomMsg_; /**< Odometry in the odom frame picked up by subscription */
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-10-24 23:56:43 UTC (rev 5770)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-10-24 23:57:40 UTC (rev 5771)
@@ -35,7 +35,7 @@
#include <MoveBase.hh>
#include <std_msgs/BaseVel.h>
-#include <std_msgs/PointCloudFloat32.h>
+#include <std_msgs/PointCloud.h>
#include <std_msgs/Pose2DFloat32.h>
#include <std_msgs/Polyline2D.h>
#include <std_srvs/StaticMap.h>
@@ -281,11 +281,11 @@
}
// Assemble a point cl...
[truncated message content] |
|
From: <tf...@us...> - 2008-10-27 20:50:54
|
Revision: 5819
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5819&view=rev
Author: tfoote
Date: 2008-10-27 20:50:41 +0000 (Mon, 27 Oct 2008)
Log Message:
-----------
two messages down
Modified Paths:
--------------
pkg/trunk/nav/fake_localization/fake_localization.cpp
Removed Paths:
-------------
pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg
pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-10-27 20:41:44 UTC (rev 5818)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-10-27 20:50:41 UTC (rev 5819)
@@ -54,7 +54,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "base_pose_ground_truth"/Pose3DEulerFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "base_pose_ground_truth"/std_msgs::TransformWithRateStamped : robot's odometric pose. Only the position information is used (velocity is ignored).
- @b "initialpose"/Pose2DFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
Publishes to (name / type):
@@ -77,7 +77,6 @@
#include <std_msgs/Pose3DStamped.h>
#include <std_msgs/ParticleCloud2D.h>
#include <std_msgs/Pose2DFloat32.h>
-#include <robot_msgs/Pose3DEulerFloat32.h>
#include <math_utils/angles.h>
#include <rosTF/rosTF.h>
Deleted: pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg 2008-10-27 20:41:44 UTC (rev 5818)
+++ pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg 2008-10-27 20:50:41 UTC (rev 5819)
@@ -1,3 +0,0 @@
-Header header
-std_msgs/Point32 position
-std_msgs/EulerAnglesFloat32 orientation
Deleted: pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg 2008-10-27 20:41:44 UTC (rev 5818)
+++ pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg 2008-10-27 20:50:41 UTC (rev 5819)
@@ -1,3 +0,0 @@
-Header header
-std_msgs/Position position
-std_msgs/Orientation orientation
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-27 22:36:07
|
Revision: 5822
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5822&view=rev
Author: gerkey
Date: 2008-10-27 21:32:51 +0000 (Mon, 27 Oct 2008)
Log Message:
-----------
Reverted changes from r5816, backdating opencv due to conflicts on update.
Blacklisted stereo_blob_tracker, as it appears to need the newer opencv
content.
Modified Paths:
--------------
pkg/trunk/3rdparty/opencv_latest/Makefile
Added Paths:
-----------
pkg/trunk/vision/stereo_blob_tracker/ROS_BUILD_BLACKLIST
Modified: pkg/trunk/3rdparty/opencv_latest/Makefile
===================================================================
--- pkg/trunk/3rdparty/opencv_latest/Makefile 2008-10-27 21:26:26 UTC (rev 5821)
+++ pkg/trunk/3rdparty/opencv_latest/Makefile 2008-10-27 21:32:51 UTC (rev 5822)
@@ -1,6 +1,6 @@
CVS_DIR = opencv-cvs
INSTALL_DIR = opencv
-CVS_DATE = "10/26/2008 00:00"
+CVS_DATE = "10/12/2008 00:00"
all: $(INSTALL_DIR)
Added: pkg/trunk/vision/stereo_blob_tracker/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/stereo_blob_tracker/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/stereo_blob_tracker/ROS_BUILD_BLACKLIST 2008-10-27 21:32:51 UTC (rev 5822)
@@ -0,0 +1 @@
+Requires newer version of opencv
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-28 15:13:58
|
Revision: 5861
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5861&view=rev
Author: gerkey
Date: 2008-10-28 15:13:53 +0000 (Tue, 28 Oct 2008)
Log Message:
-----------
fixes for gcc 4.3
Modified Paths:
--------------
pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.cpp
pkg/trunk/nav/wavefront_player/cli.cpp
Modified: pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.cpp
===================================================================
--- pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.cpp 2008-10-28 15:11:56 UTC (rev 5860)
+++ pkg/trunk/drivers/laser/hokuyo_driver/hokuyo.cpp 2008-10-28 15:13:53 UTC (rev 5861)
@@ -20,6 +20,7 @@
#include <stdio.h>
+#include <string.h>
#include <errno.h>
#include <termios.h>
#include <math.h>
Modified: pkg/trunk/nav/wavefront_player/cli.cpp
===================================================================
--- pkg/trunk/nav/wavefront_player/cli.cpp 2008-10-28 15:11:56 UTC (rev 5860)
+++ pkg/trunk/nav/wavefront_player/cli.cpp 2008-10-28 15:13:53 UTC (rev 5861)
@@ -1,4 +1,5 @@
#include <cstdio>
+#include <stdlib.h>
#include "ros/node.h"
#include "std_msgs/Planner2DState.h"
#include "std_msgs/Planner2DGoal.h"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-10-28 22:44:12
|
Revision: 5891
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5891&view=rev
Author: stuglaser
Date: 2008-10-28 22:44:05 +0000 (Tue, 28 Oct 2008)
Log Message:
-----------
Changed SetVectorCommand service to SetVector. Moved SetVector and GetVector into robot_srvs
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
Added Paths:
-----------
pkg/trunk/robot_srvs/srv/GetVector.srv
pkg/trunk/robot_srvs/srv/SetVector.srv
Removed Paths:
-------------
pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv
pkg/trunk/controllers/robot_mechanism_controllers/srv/SetVectorCommand.srv
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h 2008-10-28 22:43:55 UTC (rev 5890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h 2008-10-28 22:44:05 UTC (rev 5891)
@@ -44,7 +44,7 @@
#include <vector>
#include "ros/node.h"
-#include "robot_mechanism_controllers/SetVectorCommand.h"
+#include "robot_srvs/SetVector.h"
#include "mechanism_model/controller.h"
#include "tf/transform_datatypes.h"
#include "misc_utils/advertised_service_guard.h"
@@ -76,8 +76,8 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
void update();
- bool setCommand(robot_mechanism_controllers::SetVectorCommand::request &req,
- robot_mechanism_controllers::SetVectorCommand::response &resp);
+ bool setCommand(robot_srvs::SetVector::request &req,
+ robot_srvs::SetVector::response &resp);
private:
CartesianEffortController c_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h 2008-10-28 22:43:55 UTC (rev 5890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h 2008-10-28 22:44:05 UTC (rev 5891)
@@ -46,8 +46,8 @@
#include <vector>
#include "ros/node.h"
-#include "robot_mechanism_controllers/SetVectorCommand.h"
-#include "robot_mechanism_controllers/GetVector.h"
+#include "robot_srvs/SetVector.h"
+#include "robot_srvs/GetVector.h"
#include "robot_mechanism_controllers/cartesian_effort_controller.h"
#include "control_toolbox/pid.h"
#include "mechanism_model/controller.h"
@@ -89,10 +89,10 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
void update();
- bool setCommand(robot_mechanism_controllers::SetVectorCommand::request &req,
- robot_mechanism_controllers::SetVectorCommand::response &resp);
- bool getActual(robot_mechanism_controllers::GetVector::request &req,
- robot_mechanism_controllers::GetVector::response &resp);
+ bool setCommand(robot_srvs::SetVector::request &req,
+ robot_srvs::SetVector::response &resp);
+ bool getActual(robot_srvs::GetVector::request &req,
+ robot_srvs::GetVector::response &resp);
void command();
private:
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_velocity_controller.h 2008-10-28 22:43:55 UTC (rev 5890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_velocity_controller.h 2008-10-28 22:44:05 UTC (rev 5891)
@@ -46,8 +46,8 @@
#include <vector>
#include "ros/node.h"
-#include "robot_mechanism_controllers/SetVectorCommand.h"
-#include "robot_mechanism_controllers/GetVector.h"
+#include "robot_srvs/SetVector.h"
+#include "robot_srvs/GetVector.h"
#include "robot_mechanism_controllers/cartesian_effort_controller.h"
#include "control_toolbox/pid.h"
#include "mechanism_model/controller.h"
@@ -89,10 +89,10 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
void update();
- bool setCommand(robot_mechanism_controllers::SetVectorCommand::request &req,
- robot_mechanism_controllers::SetVectorCommand::response &resp);
- bool getActual(robot_mechanism_controllers::GetVector::request &req,
- robot_mechanism_controllers::GetVector::response &resp);
+ bool setCommand(robot_srvs::SetVector::request &req,
+ robot_srvs::SetVector::response &resp);
+ bool getActual(robot_srvs::GetVector::request &req,
+ robot_srvs::GetVector::response &resp);
void command();
private:
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp 2008-10-28 22:43:55 UTC (rev 5890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp 2008-10-28 22:44:05 UTC (rev 5891)
@@ -208,10 +208,10 @@
}
bool CartesianEffortControllerNode::setCommand(
- robot_mechanism_controllers::SetVectorCommand::request &req,
- robot_mechanism_controllers::SetVectorCommand::response &resp)
+ robot_srvs::SetVector::request &req,
+ robot_srvs::SetVector::response &resp)
{
- c_.command_ = btVector3(req.x, req.y, req.z);
+ tf::Vector3MsgToTF(req.v, c_.command_);
return true;
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp 2008-10-28 22:43:55 UTC (rev 5890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp 2008-10-28 22:44:05 UTC (rev 5891)
@@ -169,16 +169,16 @@
}
bool CartesianPositionControllerNode::setCommand(
- robot_mechanism_controllers::SetVectorCommand::request &req,
- robot_mechanism_controllers::SetVectorCommand::response &resp)
+ robot_srvs::SetVector::request &req,
+ robot_srvs::SetVector::response &resp)
{
- c_.command_ = btVector3(req.x, req.y, req.z);
+ tf::Vector3MsgToTF(req.v, c_.command_);
return true;
}
bool CartesianPositionControllerNode::getActual(
- robot_mechanism_controllers::GetVector::request &req,
- robot_mechanism_controllers::GetVector::response &resp)
+ robot_srvs::GetVector::request &req,
+ robot_srvs::GetVector::response &resp)
{
btVector3 v;
c_.getTipPosition(&v);
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-28 22:43:55 UTC (rev 5890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp 2008-10-28 22:44:05 UTC (rev 5891)
@@ -167,16 +167,18 @@
}
bool CartesianVelocityControllerNode::setCommand(
- robot_mechanism_controllers::SetVectorCommand::request &req,
- robot_mechanism_controllers::SetVectorCommand::response &resp)
+ robot_srvs::SetVector::request &req,
+ robot_srvs::SetVector::response &resp)
{
- c_.command_.set(tf::Vector3(req.x, req.y, req.z));
+ tf::Vector3 command;
+ tf::Vector3MsgToTF(req.v, command);
+ c_.command_.set(command);
return true;
}
bool CartesianVelocityControllerNode::getActual(
- robot_mechanism_controllers::GetVector::request &req,
- robot_mechanism_controllers::GetVector::response &resp)
+ robot_srvs::GetVector::request &req,
+ robot_srvs::GetVector::response &resp)
{
btVector3 v;
c_.getTipVelocity(&v);
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py 2008-10-28 22:43:55 UTC (rev 5890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py 2008-10-28 22:44:05 UTC (rev 5891)
@@ -22,7 +22,7 @@
def set_controller_vector(controller, command):
rospy.wait_for_service(controller + '/set_command')
- s = rospy.ServiceProxy(controller + '/set_command', SetVectorCommand)
+ s = rospy.ServiceProxy(controller + '/set_command', SetVector)
resp = s(*command)
def get_controller(controller):
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv 2008-10-28 22:43:55 UTC (rev 5890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv 2008-10-28 22:44:05 UTC (rev 5891)
@@ -1,2 +0,0 @@
----
-std_msgs/Vector3 v
\ No newline at end of file
Deleted: pkg/trunk/controllers/robot_mechanism_controllers/srv/SetVectorCommand.srv
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/srv/SetVectorCommand.srv 2008-10-28 22:43:55 UTC (rev 5890)
+++ pkg/trunk/controllers/robot_mechanism_controllers/srv/SetVectorCommand.srv 2008-10-28 22:44:05 UTC (rev 5891)
@@ -1,4 +0,0 @@
-float64 x
-float64 y
-float64 z
----
\ No newline at end of file
Copied: pkg/trunk/robot_srvs/srv/GetVector.srv (from rev 5889, pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv)
===================================================================
--- pkg/trunk/robot_srvs/srv/GetVector.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/GetVector.srv 2008-10-28 22:44:05 UTC (rev 5891)
@@ -0,0 +1,2 @@
+---
+std_msgs/Vector3 v
\ No newline at end of file
Added: pkg/trunk/robot_srvs/srv/SetVector.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/SetVector.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/SetVector.srv 2008-10-28 22:44:05 UTC (rev 5891)
@@ -0,0 +1,2 @@
+std_msgs/Vector3 v
+---
\ 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: <hsu...@us...> - 2008-10-29 00:03:01
|
Revision: 5899
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5899&view=rev
Author: hsujohnhsu
Date: 2008-10-29 00:02:57 +0000 (Wed, 29 Oct 2008)
Log Message:
-----------
* added demo launch files for pr2, arm, pr2_prototype
* clean up unneeded gazebo_robot_description file links in world/
* removed unused actuators.xml include from pr2.xml
* added needed files for pr2_prototype1 so gazebo runs.
Modified Paths:
--------------
pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description/pr2/backup/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_base_lab.xml
Added Paths:
-----------
pkg/trunk/demos/arm_gazebo/
pkg/trunk/demos/arm_gazebo/arm.xml
pkg/trunk/demos/arm_gazebo/arm_headless.xml
pkg/trunk/demos/pr2_gazebo/
pkg/trunk/demos/pr2_gazebo/pr2.xml
pkg/trunk/demos/pr2_gazebo/pr2_headless.xml
pkg/trunk/demos/pr2_gazebo/pr2_wg.xml
pkg/trunk/demos/pr2_gazebo/pr2_wg_headless.xml
pkg/trunk/demos/pr2_prototype1_gazebo/
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/controllers_gazebo_prototype1.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_prototype1.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_gazebo_prototype1.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/gazebo_joints_prototype1.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/pr2_prototype1.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/pr2_prototype1_gazebo_actuators.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_arm.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_dual_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_multi_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_single_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_arm.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_collision.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_dual_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_multi_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_single_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_arm.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_dual_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_multi_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_single_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2d2.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/transmissions.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/transmissions_arm.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/transmissions_dual_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/transmissions_multi_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/transmissions_single_link.xml
Added: pkg/trunk/demos/arm_gazebo/arm.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/arm.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1,15 @@
+<launch>
+ <master auto="start"/>
+ <group name="wg">
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_arm_test/pr2_arm.xml"" />
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_arm_test.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_arm_test/controllers_arm.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set gripper_left_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+ </group>
+</launch>
+
Added: pkg/trunk/demos/arm_gazebo/arm_headless.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm_headless.xml (rev 0)
+++ pkg/trunk/demos/arm_gazebo/arm_headless.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1,17 @@
+<launch>
+ <group name="wg">
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_arm_test/pr2_arm.xml"" />
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-g -n $(find gazebo_robot_description)/world/robot_arm_test_headless.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_arm_test/controllers_arm.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set gripper_left_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+ <!-- for visualization -->
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
+ </group>
+</launch>
+
Added: pkg/trunk/demos/pr2_gazebo/pr2.xml
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2.xml (rev 0)
+++ pkg/trunk/demos/pr2_gazebo/pr2.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1,22 @@
+<launch>
+ <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
+ <!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
+ <group name="wg">
+ <!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
+ (Mechanism Control, BaseControllerNode, etc...) -->
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ <!-- use mech.py to spawn all controllers listed in controllers.xml -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
+ for details of the profile, rates, see controller::LaserScannerControllerNode. -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
+ </group>
+</launch>
+
Added: pkg/trunk/demos/pr2_gazebo/pr2_headless.xml
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_headless.xml (rev 0)
+++ pkg/trunk/demos/pr2_gazebo/pr2_headless.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1,15 @@
+<launch>
+ <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
+ <group name="wg">
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <node pkg="gazebo" type="gazebo" args="-g -n $(find gazebo_robot_description)/world/robot_headless.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
+ </group>
+</launch>
+
Added: pkg/trunk/demos/pr2_gazebo/pr2_wg.xml
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg.xml (rev 0)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1,15 @@
+<launch>
+ <group name="wg">
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_wg.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
+ </group>
+</launch>
+
Added: pkg/trunk/demos/pr2_gazebo/pr2_wg_headless.xml
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg_headless.xml (rev 0)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg_headless.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1,15 @@
+<launch>
+ <group name="wg">
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-n -g $(find gazebo_robot_description)/world/robot_wg_headless.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
+ </group>
+</launch>
+
Added: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.xml
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.xml (rev 0)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1,23 @@
+<launch>
+ <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
+ <!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
+ <group name="wg">
+ <!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
+ (Mechanism Control, BaseControllerNode, etc...) -->
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_prototype1/gazebo/pr2_prototype1.xml"" />
+ <!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_prototype1.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ <!-- use mech.py to spawn all controllers listed in controllers.xml -->
+ <param name="base_controller/odom_publish_rate" value="30.0"/>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_base_lab.xml" output="screen"/>
+ <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
+ for details of the profile, rates, see controller::LaserScannerControllerNode. -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
+ </group>
+</launch>
+
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-10-29 00:02:57 UTC (rev 5899)
@@ -63,4 +63,10 @@
DEPENDS urdf2gazebo
COMMENT "Building Gazebo model for Multi Link Test")
+# pr2_prototype1
+add_custom_target(pr2_prototype1 ALL
+ COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2_prototype1/gazebo/pr2_prototype1_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_prototype1.model
+ DEPENDS urdf2gazebo
+ COMMENT "Building Gazebo model for PR2 Prototype1 Test")
+
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2/actuators.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_arm.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_arm.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_arm.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2_arm_test/actuators_arm.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_dual_link.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_dual_link.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/dual_link_test/actuators_dual_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_multi_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_multi_link.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_multi_link.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/multi_link_test/actuators_multi_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_single_link.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/actuators_single_link.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/single_link_test/actuators_single_link.xml
\ No newline at end of file
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/controllers_gazebo_prototype1.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/controllers_gazebo_prototype1.xml (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/controllers_gazebo_prototype1.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1 @@
+link ../../wg_robot_description/pr2_prototype1/gazebo/controllers_gazebo_prototype1.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/gazebo_robot_description/world/controllers_gazebo_prototype1.xml
___________________________________________________________________
Added: svn:special
+ *
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_prototype1.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_prototype1.xml (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_prototype1.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1 @@
+link ../../wg_robot_description/pr2_prototype1/gazebo/gazebo_joints_prototype1.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_prototype1.xml
___________________________________________________________________
Added: svn:special
+ *
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2/groups.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_arm.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_arm.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_arm.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2_arm_test/groups_arm.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_collision.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_collision.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_collision.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2/groups_collision.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_dual_link.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_dual_link.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/dual_link_test/groups_dual_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_multi_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_multi_link.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_multi_link.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/multi_link_test/groups_multi_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_single_link.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/groups_single_link.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/single_link_test/groups_single_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2/pr2.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_arm.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_arm.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_arm.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2_arm_test/pr2_arm.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_dual_link.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_dual_link.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/dual_link_test/pr2_dual_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_multi_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_multi_link.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_multi_link.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/multi_link_test/pr2_multi_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_single_link.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_single_link.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/single_link_test/pr2_single_link.xml
\ No newline at end of file
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2d2.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2d2.xml 2008-10-28 23:51:53 UTC (rev 5898)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2d2.xml 2008-10-29 00:02:57 UTC (rev 5899)
@@ -1 +0,0 @@
-link ../../wg_robot_description/pr2_arm/pr2d2.xml
\ No newline at end of file
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-10-29 00:02:57 UTC (rev 5899)
@@ -0,0 +1,349 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
+
+ <verbosity>5</verbosity>
+
+<!-- cfm is 1e-5 for single precision -->
+<!-- erp is typically .1-.8 -->
+<!-- here's the global contact cfm/erp -->
+ <physics:ode>
+ <stepTime>0.01</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.0000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>false</quickStep>
+ </physics:ode>
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <size>1024 800</size>
+ <pos>0 0</pos>
+ <frames>
+ <row height="100%">
+ <camera width="100%">
+ <xyz>0 0 20</xyz>
+ <rpy>0 90 90</rpy>
+ </camera>
+ </row>
+ </frames>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>false</grid>
+ <maxUpdateRate>100</maxUpdateRate>
+ </rendering:ogre>
+
+
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <!-- map3.png -->
+ <material>PR2/floor_texture</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+ <!--
+ <model:empty name="ros_model">
+ <body:empty name="ros_body">
+ <controller:ros_node name="ros_node" plugin="libRos_Node.so">
+ <nodeName>simulator_ros_node</nodeName>
+ </controller:ros_node>
+ </body:empty>
+ </model:empty>
+ -->
+
+ <!-- The "desk"-->
+ <model:physical name="desk1_model">
+ <xyz> 2.28 -0.21 -0.10</xyz>
+ <rpy> 0.0 0.0 0.00</rpy>
+ <body:box name="desk1_legs_body">
+ <geom:box name="desk1_legs_geom">
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <xyz> 0.0 0.0 0.50</xyz>
+ <mesh>default</mesh>
+ <size>0.5 1.0 0.75</size>
+ <mass> 10.0</mass>
+ <visual>
+ <size> 0.5 1.0 0.75</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ <geom:box name="desk1_top_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <xyz> 0.0 0.0 0.90</xyz>
+ <mesh>default</mesh>
+ <size>0.75 1.5 0.05</size>
+ <mass> 10.0</mass>
+ <visual>
+ <size> 0.75 1.5 0.05</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+...
[truncated message content] |
|
From: <jfa...@us...> - 2008-10-29 18:22:22
|
Revision: 5922
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5922&view=rev
Author: jfaustwg
Date: 2008-10-29 18:22:01 +0000 (Wed, 29 Oct 2008)
Log Message:
-----------
r10894@lan-dhcp-121: jfaust | 2008-10-28 11:38:36 -0700
First pass at fixed-frame transforms of fixed data, and relative transforms of only the camera + robot-centric things
Modified Paths:
--------------
pkg/trunk/visualization/ogre_tools/CMakeLists.txt
pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h
pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h
pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.h
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:10894
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_tools/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/ogre_tools/CMakeLists.txt 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/CMakeLists.txt 2008-10-29 18:22:01 UTC (rev 5922)
@@ -2,8 +2,8 @@
include(rosbuild)
include(FindPkgConfig)
-set(ROS_BUILD_TYPE RelWithDebInfo)
-#set(ROS_BUILD_TYPE Debug)
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+set(ROS_BUILD_TYPE Debug)
rospack(ogre_tools)
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -43,6 +43,7 @@
CameraBase::CameraBase( Ogre::SceneManager* scene_manager )
: scene_manager_( scene_manager )
+, relative_node_( NULL )
{
std::stringstream ss;
static uint32_t count = 0;
@@ -55,6 +56,15 @@
scene_manager_->destroyCamera( camera_ );
}
+void CameraBase::setRelativeNode( Ogre::SceneNode* node )
+{
+ relative_node_ = node;
+
+ relativeNodeChanged();
+
+ update();
+}
+
void CameraBase::setPosition( const Ogre::Vector3& position )
{
setPosition( position.x, position.y, position.z );
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/camera_base.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -65,6 +65,12 @@
* @return The Ogre camera associated with this camera object
*/
Ogre::Camera* getOgreCamera() { return camera_; }
+
+ void setRelativeNode( Ogre::SceneNode* node );
+ virtual void relativeNodeChanged() {}
+
+ virtual void update() = 0;
+
/**
* \brief Set the position of the camera
*/
@@ -188,6 +194,8 @@
protected:
Ogre::Camera* camera_; ///< Ogre camera associated with this camera object
Ogre::SceneManager* scene_manager_; ///< Scene manager this camera is part of
+
+ Ogre::SceneNode* relative_node_;
};
} // namespace ogre_tools
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -53,6 +53,14 @@
{
}
+void FPSCamera::relativeNodeChanged()
+{
+ if ( relative_node_ )
+ {
+ relative_node_->attachObject( camera_ );
+ }
+}
+
void FPSCamera::update()
{
Ogre::Matrix3 pitch, yaw;
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/fps_camera.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -58,6 +58,7 @@
virtual void setOrientation( float x, float y, float z, float w );
virtual void setPosition( float x, float y, float z );
virtual void setFrom( CameraBase* camera );
+ virtual void relativeNodeChanged();
virtual Ogre::Vector3 getPosition();
virtual Ogre::Quaternion getOrientation();
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -41,7 +41,7 @@
namespace ogre_tools
{
-Grid::Grid( Ogre::SceneManager* scene_manager, uint32_t gridSize, float cell_length, float r, float g, float b )
+Grid::Grid( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, uint32_t gridSize, float cell_length, float r, float g, float b )
: scene_manager_( scene_manager )
{
static uint32_t gridCount = 0;
@@ -50,7 +50,12 @@
manual_object_ = scene_manager_->createManualObject( ss.str() );
- scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+ if ( !parent_node )
+ {
+ parent_node = scene_manager_->getRootSceneNode();
+ }
+
+ scene_node_ = parent_node->createChildSceneNode();
scene_node_->attachObject( manual_object_ );
set( gridSize, cell_length, r, g, b );
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/grid.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -66,7 +66,7 @@
* @param g Green color component, in the range [0, 1]
* @param b Blue color component, in the range [0, 1]
*/
- Grid( Ogre::SceneManager* manager, uint32_t cell_count, float cell_length, float r, float g, float b );
+ Grid( Ogre::SceneManager* manager, Ogre::SceneNode* parent_node, uint32_t cell_count, float cell_length, float r, float g, float b );
~Grid();
/**
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -51,7 +51,7 @@
OrbitCamera::OrbitCamera( Ogre::SceneManager* scene_manager )
: CameraBase( scene_manager )
-, focal_point_( 0.0f, 0.0f, 0.0f )
+, focal_point_( Ogre::Vector3::ZERO )
, yaw_( YAW_START )
, pitch_( PITCH_START )
, distance_( 10.0f )
@@ -87,12 +87,27 @@
void OrbitCamera::update()
{
- float x = distance_ * cos( yaw_ ) * sin( pitch_ ) + focal_point_.x;
- float y = distance_ * cos( pitch_ ) + focal_point_.y;
- float z = distance_ * sin( yaw_ ) * sin( pitch_ ) + focal_point_.z;
+ Ogre::Vector3 global_focal_point = focal_point_;
- camera_->setPosition( x, y, z );
- camera_->lookAt( focal_point_ );
+ if ( relative_node_ )
+ {
+ global_focal_point = relative_node_->getOrientation() * focal_point_ + relative_node_->getPosition();
+ }
+
+ float x = distance_ * cos( yaw_ ) * sin( pitch_ ) + global_focal_point.x;
+ float y = distance_ * cos( pitch_ ) + global_focal_point.y;
+ float z = distance_ * sin( yaw_ ) * sin( pitch_ ) + global_focal_point.z;
+
+ Ogre::Vector3 pos( x, y, z );
+
+ if ( relative_node_ )
+ {
+ Ogre::Vector3 vec = pos - global_focal_point;
+ pos = relative_node_->getOrientation() * vec + global_focal_point;
+ }
+
+ camera_->setPosition( pos );
+ camera_->lookAt( global_focal_point );
}
void OrbitCamera::yaw( float angle )
@@ -202,8 +217,14 @@
void OrbitCamera::move( float x, float y, float z )
{
- focal_point_ += camera_->getOrientation() * Ogre::Vector3( x, y, z );
+ Ogre::Quaternion orientation = camera_->getOrientation();
+ if ( relative_node_ )
+ {
+ orientation = relative_node_->getOrientation() * orientation;
+ }
+ focal_point_ += orientation * Ogre::Vector3( x, y, z );
+
update();
}
@@ -219,9 +240,21 @@
void OrbitCamera::lookAt( const Ogre::Vector3& point )
{
- distance_ = point.distance( camera_->getPosition() );
- focal_point_ = point;
+ Ogre::Vector3 focal_point = point;
+ Ogre::Vector3 camera_position = camera_->getPosition();
+ if ( relative_node_ )
+ {
+ Ogre::Vector3 rel_pos = relative_node_->getPosition();
+ Ogre::Quaternion rel_orient = relative_node_->getOrientation();
+
+ focal_point = rel_orient.Inverse() * ( focal_point - rel_pos );
+ camera_position = rel_orient.Inverse() * ( camera_position - rel_pos );
+ }
+
+ distance_ = focal_point.distance( camera_position );
+ focal_point_ = focal_point;
+
update();
}
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -92,11 +92,13 @@
virtual void mouseRightDrag( int diff_x, int diff_y );
virtual void scrollWheel( int diff );
-private:
/**
* \brief Calculates the camera's position and orientation from the yaw, pitch, distance and focal point
*/
- void update();
+ virtual void update();
+
+private:
+
/**
* \brief Calculates pitch and yaw values given a new position and the current focal point
* @param position Position to calculate the pitch/yaw for
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/ortho_camera.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -45,7 +45,7 @@
OrthoCamera( wxOgreRenderWindow* render_window, Ogre::SceneManager* scene_manager );
virtual ~OrthoCamera();
- void update();
+ virtual void update();
virtual void setFrom( CameraBase* camera );
virtual void yaw( float angle );
Modified: pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_tools/src/test/grid_test.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -74,7 +74,7 @@
m_WXRenderWindow->getViewport()->setCamera( camera_->getOgreCamera() );
- ogre_tools::Grid* grid = new ogre_tools::Grid( scene_manager_, 10, 1.0f, 1.0f, 0.0f, 0.0f );
+ ogre_tools::Grid* grid = new ogre_tools::Grid( scene_manager_, NULL, 10, 1.0f, 1.0f, 0.0f, 0.0f );
//grid->getSceneNode()->pitch( Ogre::Degree( 90 ) );
//ogre_tools::Axes* axes = new ogre_tools::Axes( scene_manager_ );
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -83,6 +83,9 @@
scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
ray_scene_query_ = scene_manager_->createRayQuery( Ogre::Ray() );
+ target_relative_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
+ updateRelativeNode();
+
selection_bounds_particle_system_ = scene_manager_->createParticleSystem( "VisualizationManagerSelectionBoundsParticleSystem" );
selection_bounds_particle_system_->setMaterialName( "BaseWhiteNoLighting" );
Ogre::SceneNode* node = scene_manager_->getRootSceneNode()->createChildSceneNode();
@@ -101,10 +104,13 @@
property_manager_ = new PropertyManager( vis_panel_->getPropertyGrid() );
CategoryProperty* category = property_manager_->createCategory( "Global Options", "", NULL );
- coordinate_frame_property_ = property_manager_->createProperty<StringProperty>( "Coordinate Frame", "", boost::bind( &VisualizationManager::getCoordinateFrame, this ),
- boost::bind( &VisualizationManager::setCoordinateFrame, this, _1 ), category );
+ target_frame_property_ = property_manager_->createProperty<StringProperty>( "Target Frame", "", boost::bind( &VisualizationManager::getTargetFrame, this ),
+ boost::bind( &VisualizationManager::setTargetFrame, this, _1 ), category );
+ fixed_frame_property_ = property_manager_->createProperty<StringProperty>( "Fixed Frame", "", boost::bind( &VisualizationManager::getFixedFrame, this ),
+ boost::bind( &VisualizationManager::setFixedFrame, this, _1 ), category );
- setCoordinateFrame( "base" );
+ setTargetFrame( "base" );
+ setFixedFrame( "map" );
}
VisualizationManager::~VisualizationManager()
@@ -184,6 +190,10 @@
vis_panel_->queueRender();
render_update_time = 0.0f;
}
+
+ updateRelativeNode();
+
+ vis_panel_->getCurrentCamera()->update();
}
void VisualizationManager::addVisualizer( VisualizerBase* visualizer, bool allow_deletion, bool enabled )
@@ -198,6 +208,7 @@
visualizer->setUnlockRenderCallback( boost::bind( &VisualizationPanel::unlockRender, vis_panel_ ) );
visualizer->setTargetFrame( target_frame_ );
+ visualizer->setFixedFrame( fixed_frame_ );
vis_panel_->getPropertyGrid()->Freeze();
@@ -476,7 +487,7 @@
removeVisualizer( visualizer );
}
-void VisualizationManager::setCoordinateFrame( const std::string& frame )
+void VisualizationManager::setTargetFrame( const std::string& frame )
{
target_frame_ = frame;
@@ -489,9 +500,25 @@
visualizer->setTargetFrame(frame);
}
- coordinate_frame_property_->changed();
+ target_frame_property_->changed();
}
+void VisualizationManager::setFixedFrame( const std::string& frame )
+{
+ fixed_frame_ = frame;
+
+ V_VisualizerInfo::iterator it = visualizers_.begin();
+ V_VisualizerInfo::iterator end = visualizers_.end();
+ for ( ; it != end; ++it )
+ {
+ VisualizerBase* visualizer = it->visualizer_;
+
+ visualizer->setFixedFrame(frame);
+ }
+
+ fixed_frame_property_->changed();
+}
+
bool VisualizationManager::isDeletionAllowed( VisualizerBase* visualizer )
{
VisualizerInfo* info = getVisualizerInfo( visualizer );
@@ -520,4 +547,32 @@
}
}
+void VisualizationManager::updateRelativeNode()
+{
+ tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0.0f, 0.0f, 0.0f ), btVector3( 0.0f, 0.0f, 0.0f ) ),
+ ros::Time(0), target_frame_ );
+
+ try
+ {
+ tf_->transformPose( fixed_frame_, pose, pose );
+ }
+ catch(tf::TransformException& e)
+ {
+ ROS_ERROR( "Error transforming from frame '%s' to frame '%s': %s\n", target_frame_.c_str(), fixed_frame_.c_str() );
+ }
+
+ Ogre::Vector3 position = Ogre::Vector3( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
+ robotToOgre( position );
+
+ btQuaternion quat;
+ pose.getBasis().getRotation( quat );
+ Ogre::Quaternion orientation( Ogre::Quaternion::IDENTITY );
+ ogreToRobot( orientation );
+ orientation = Ogre::Quaternion( quat.w(), quat.x(), quat.y(), quat.z() ) * orientation;
+ robotToOgre( orientation );
+
+ target_relative_node_->setPosition( position );
+ target_relative_node_->setOrientation( orientation );
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -51,6 +51,7 @@
{
class Root;
class SceneManager;
+class SceneNode;
class Camera;
class RaySceneQuery;
class ParticleSystem;
@@ -160,10 +161,17 @@
* \brief Set the coordinate frame we should be displaying in
* @param frame The string name -- must match the frame name broadcast to libTF
*/
- void setCoordinateFrame( const std::string& frame );
- const std::string& getCoordinateFrame() { return target_frame_; }
+ void setTargetFrame( const std::string& frame );
+ const std::string& getTargetFrame() { return target_frame_; }
/**
+ * \brief Set the coordinate frame we should be transforming all fixed data to
+ * @param frame The string name -- must match the frame name broadcast to libTF
+ */
+ void setFixedFrame( const std::string& frame );
+ const std::string& getFixedFrame() { return fixed_frame_; }
+
+ /**
* \brief Performs a linear search to find a visualizer based on its name
* @param name Name of the visualizer to search for
*/
@@ -191,6 +199,8 @@
VisualizerSignal& getVisualizerStateSignal() { return visualizer_state_; }
+ Ogre::SceneNode* getTargetRelativeNode() { return target_relative_node_; }
+
protected:
/**
* \brief Add a visualizer to be managed by this panel
@@ -208,6 +218,8 @@
/// Called from the update timer
void onUpdate( wxTimerEvent& event );
+ void updateRelativeNode();
+
Ogre::Root* ogre_root_; ///< Ogre Root
Ogre::SceneManager* scene_manager_; ///< Ogre scene manager associated with this panel
Ogre::RaySceneQuery* ray_scene_query_; ///< Used for querying the scene based on the mouse location
@@ -234,13 +246,17 @@
M_Factory factories_; ///< Factories by visualizer type name
std::string target_frame_; ///< Target coordinate frame we're displaying everything in
+ std::string fixed_frame_; ///< Frame to transform fixed data to
PropertyManager* property_manager_;
- StringProperty* coordinate_frame_property_;
+ StringProperty* target_frame_property_;
+ StringProperty* fixed_frame_property_;
VisualizationPanel* vis_panel_;
VisualizerSignal visualizer_state_;
+
+ Ogre::SceneNode* target_relative_node_;
};
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -120,14 +120,17 @@
fps_camera_ = new ogre_tools::FPSCamera( manager_->getSceneManager() );
fps_camera_->getOgreCamera()->setNearClipDistance( 0.1f );
fps_camera_->setPosition( 0, 0, 15 );
+ fps_camera_->setRelativeNode( manager_->getTargetRelativeNode() );
orbit_camera_ = new ogre_tools::OrbitCamera( manager_->getSceneManager() );
orbit_camera_->getOgreCamera()->setNearClipDistance( 0.1f );
orbit_camera_->setPosition( 0, 0, 15 );
+ orbit_camera_->setRelativeNode( manager_->getTargetRelativeNode() );
top_down_ortho_ = new ogre_tools::OrthoCamera( render_panel_, manager_->getSceneManager() );
top_down_ortho_->setPosition( 0, 30, 0 );
top_down_ortho_->pitch( -Ogre::Math::HALF_PI );
+ top_down_ortho_->setRelativeNode( manager_->getTargetRelativeNode() );
current_camera_ = orbit_camera_;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -131,6 +131,16 @@
}
}
+void VisualizerBase::setFixedFrame( const std::string& frame )
+{
+ fixed_frame_ = frame;
+
+ if ( isEnabled() )
+ {
+ fixedFrameChanged();
+ }
+}
+
void VisualizerBase::setPropertyManager( PropertyManager* manager, CategoryProperty* parent )
{
property_manager_ = manager;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -127,7 +127,7 @@
*/
virtual void createProperties() {}
- /// Set the target frame of this visualizer. This is a frame id which should match something being broadcast through libTF.
+ /// Set the target frame of this visualizer. This is a frame id which should match something being broadcast through TF.
void setTargetFrame( const std::string& frame );
/**
@@ -136,6 +136,17 @@
virtual void targetFrameChanged() = 0;
/**
+ * \brief Set the fixed frame of this visualizer. This is a frame id which should generally be the top-level frame being broadcast through TF
+ * @param frame The fixed frame
+ */
+ void setFixedFrame( const std::string& frame );
+
+ /**
+ * \brief Called from within setFixedFrame, notifying child classes that the fixed frame has changed
+ */
+ virtual void fixedFrameChanged() = 0;
+
+ /**
* \brief Returns whether an object owned by this visualizer is pickable/mouse selectable
* @param object The Ogre::MovableObject to check
*/
@@ -171,14 +182,15 @@
std::string name_; ///< The name of this visualizer
bool enabled_; ///< Are we enabled?
- std::string target_frame_; ///< The frame we should transform everything into
+ std::string target_frame_; ///< The frame we should transform all periodically-updated data into
+ std::string fixed_frame_; ///< The frame we should transform all fixed data into
boost::function<void ()> render_callback_; ///< Render callback
boost::function<void ()> render_lock_; ///< Render lock callback
boost::function<void ()> render_unlock_; ///< Render unlock callback
ros::node* ros_node_; ///< ros node
- tf::TransformListener* tf_; ///< rosTF client
+ tf::TransformListener* tf_; ///< tf client
std::string property_prefix_; ///< Prefix to prepend to our properties
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -28,6 +28,7 @@
*/
#include "axes_visualizer.h"
+#include "visualization_manager.h"
#include "properties/property.h"
#include "properties/property_manager.h"
#include "common.h"
@@ -47,7 +48,7 @@
, length_( 1.0 )
, radius_( 0.1 )
{
- axes_ = new ogre_tools::Axes( scene_manager_, NULL, length_, radius_ );
+ axes_ = new ogre_tools::Axes( scene_manager_, manager->getTargetRelativeNode(), length_, radius_ );
axes_->getSceneNode()->setVisible( isEnabled() );
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/axes_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -66,6 +66,7 @@
// Overrides from VisualizerBase
virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged() {}
virtual void createProperties();
static const char* getTypeStatic() { return "Axes"; }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -28,6 +28,8 @@
*/
#include "grid_visualizer.h"
+#include "common.h"
+#include "visualization_manager.h"
#include "properties/property.h"
#include "properties/property_manager.h"
@@ -50,7 +52,11 @@
, cellsize_property_( NULL )
, color_property_( NULL )
{
- grid_ = new ogre_tools::Grid( scene_manager_, cell_count_, cell_size_, color_.r_, color_.g_, color_.b_ );
+ grid_ = new ogre_tools::Grid( scene_manager_, manager->getTargetRelativeNode(), cell_count_, cell_size_, color_.r_, color_.g_, color_.b_ );
+
+ /*Ogre::Quaternion orient( Ogre::Quaternion::IDENTITY );
+ ogreToRobot( orient );
+ grid_->getSceneNode()->setOrientation( orient );*/
}
GridVisualizer::~GridVisualizer()
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.h 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/grid_visualizer.h 2008-10-29 18:22:01 UTC (rev 5922)
@@ -96,6 +96,7 @@
// Overrides from VisualizerBase
virtual void targetFrameChanged() {}
+ virtual void fixedFrameChanged() {}
virtual void createProperties();
static const char* getTypeStatic() { return "Grid"; }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-10-29 18:00:08 UTC (rev 5921)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-10-29 18:22:01 UTC (rev 5922)
@@ -169,6 +169,17 @@
causeRender();
}
+void LaserScanVisualizer::clear()
+{
+ RenderAutoLock renderLock( this );
+
+ cloud_->setCloudVisible( false );
+ cloud_->clear();
+ points_.clear();
+ point_times_.clear();
+ cloud_messages_.clear();
+}
+
v...
[truncated message content] |
|
From: <jfa...@us...> - 2008-10-29 18:28:26
|
Revision: 5923
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5923&view=rev
Author: jfaustwg
Date: 2008-10-29 18:28:23 +0000 (Wed, 29 Oct 2008)
Log Message:
-----------
r10969@lan-dhcp-121: jfaust | 2008-10-28 13:56:49 -0700
Fixed warnings brought on by adding the __printf__ attribute in rosconsole
Modified Paths:
--------------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:10894
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:10969
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property.h 2008-10-29 18:22:01 UTC (rev 5922)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property.h 2008-10-29 18:28:23 UTC (rev 5923)
@@ -126,8 +126,8 @@
*/
virtual ~Property()
{
- PROPERTY_DEBUG("Property Destructor: 0x%08x, %s%s", this, (const char*)prefix_.mb_str(), (const char*)name_.mb_str());
- PROPERTY_DEBUG("Deleting wxPGProperty: 0x%08x", property_);
+ PROPERTY_DEBUG("Property Destructor: 0x%08x, %s%s", (int)this, (const char*)prefix_.mb_str(), (const char*)name_.mb_str());
+ PROPERTY_DEBUG("Deleting wxPGProperty: 0x%08x", (int)property_);
grid_->DeleteProperty( property_ );
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.cpp 2008-10-29 18:22:01 UTC (rev 5922)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.cpp 2008-10-29 18:28:23 UTC (rev 5923)
@@ -75,7 +75,7 @@
return;
}
- PROPERTY_DEBUG("PropertyManager::deleteProperty( PropertyBase* property ): 0x%08x, %s%s", property, property->getPrefix().c_str(), property->getName().c_str());
+ PROPERTY_DEBUG("PropertyManager::deleteProperty( PropertyBase* property ): 0x%08x, %s%s", (int)property, property->getPrefix().c_str(), property->getName().c_str());
deleteProperty( property->getName(), property->getPrefix() );
}
@@ -115,7 +115,7 @@
void PropertyManager::deleteByUserData( void* user_data )
{
- PROPERTY_DEBUG("PropertyManager::deleteByUserData: user_data=0x%08x", user_data);
+ PROPERTY_DEBUG("PropertyManager::deleteByUserData: user_data=0x%08x", (int)user_data);
std::set<PropertyBase*> to_delete;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.h 2008-10-29 18:22:01 UTC (rev 5922)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.h 2008-10-29 18:28:23 UTC (rev 5923)
@@ -79,7 +79,7 @@
std::pair<M_Property::iterator, bool> pib = properties_.insert( std::make_pair( std::make_pair(prefix, name), property ) );
ROS_ASSERT(pib.second);
- PROPERTY_DEBUG("PropertyManager::createProperty: Inserted property 0x%08x, %s%s", property, pib.first->first.first.c_str(), pib.first->first.second.c_str());
+ PROPERTY_DEBUG("PropertyManager::createProperty: Inserted property 0x%08x, %s%s", (int)property, pib.first->first.first.c_str(), pib.first->first.second.c_str());
property->writeToGrid();
property->setPGClientData();
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-10-29 18:22:01 UTC (rev 5922)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-10-29 18:28:23 UTC (rev 5923)
@@ -558,7 +558,7 @@
}
catch(tf::TransformException& e)
{
- ROS_ERROR( "Error transforming from frame '%s' to frame '%s': %s\n", target_frame_.c_str(), fixed_frame_.c_str() );
+ ROS_ERROR( "Error transforming from frame '%s' to frame '%s'\n", target_frame_.c_str(), fixed_frame_.c_str() );
}
Ogre::Vector3 position = Ogre::Vector3( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-10-29 18:28:34
|
Revision: 5924
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5924&view=rev
Author: jfaustwg
Date: 2008-10-29 18:28:31 +0000 (Wed, 29 Oct 2008)
Log Message:
-----------
r10971@lan-dhcp-121: jfaust | 2008-10-28 14:17:39 -0700
Fix orbit camera movement
Modified Paths:
--------------
pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:10969
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:10971
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2008-10-29 18:28:23 UTC (rev 5923)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2008-10-29 18:28:31 UTC (rev 5924)
@@ -218,9 +218,10 @@
void OrbitCamera::move( float x, float y, float z )
{
Ogre::Quaternion orientation = camera_->getOrientation();
+
if ( relative_node_ )
{
- orientation = relative_node_->getOrientation() * orientation;
+ orientation = relative_node_->getOrientation().Inverse() * orientation;
}
focal_point_ += orientation * Ogre::Vector3( x, y, z );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-10-29 18:29:26
|
Revision: 5925
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5925&view=rev
Author: jfaustwg
Date: 2008-10-29 18:29:17 +0000 (Wed, 29 Oct 2008)
Log Message:
-----------
r11000@lan-dhcp-121: jfaust | 2008-10-28 18:45:11 -0700
Allow the visualizer to reset itself when time jumps backwards (generally means the simulator has restarted, or a log file is being replayed).
Also misc. bug fixes
Modified Paths:
--------------
pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.cpp
pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.h
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:10971
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11000
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/point_cloud.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -104,6 +104,13 @@
point_count_ = 0;
bounding_box_.setExtents( -10000.0f, -10000.0f, -10000.0f, 10000.0f, 10000.0f, 10000.0f );
bounding_radius_ = 30000.0f;
+
+ V_BillboardSet::iterator bbs_it = billboard_sets_.begin();
+ V_BillboardSet::iterator bbs_end = billboard_sets_.end();
+ for ( ; bbs_it != bbs_end; ++bbs_it )
+ {
+ (*bbs_it)->setVisible( false );
+ }
}
void PointCloud::setCloudVisible( bool visible )
Modified: pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt 2008-10-29 18:29:17 UTC (rev 5925)
@@ -2,8 +2,8 @@
include(rosbuild)
include(FindPkgConfig)
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-set(ROS_BUILD_TYPE Debug)
+set(ROS_BUILD_TYPE RelWithDebInfo)
+#set(ROS_BUILD_TYPE Debug)
rospack(ogre_visualizer)
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -599,6 +599,10 @@
void Robot::update( tf::TransformListener* tf, const std::string& target_frame )
{
+ typedef std::vector<std::string> V_string;
+ V_string frames;
+ tf->getFrameStrings( frames );
+
M_NameToLinkInfo::iterator link_it = links_.begin();
M_NameToLinkInfo::iterator link_end = links_.end();
for ( ; link_it != link_end; ++link_it )
@@ -606,14 +610,17 @@
const std::string& name = link_it->first;
LinkInfo* info = link_it->second;
+ if ( std::find( frames.begin(), frames.end(), name ) == frames.end() )
+ {
+ ROS_ERROR( "Frame '%s' does not exist in the TF frame list", name.c_str() );
+ continue;
+ }
+
tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0, 0, 0 ), btVector3( 0, 0, 0 ) ), ros::Time(0), name );
try
{
- if ( name != target_frame )
- {
- tf->transformPose( target_frame, pose, pose );
- }
+ tf->transformPose( target_frame, pose, pose );
}
catch(tf::TransformException& e)
{
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -87,17 +87,27 @@
M_Property::iterator found_it = properties_.find( std::make_pair( prefix, name ) );
ROS_ASSERT( found_it != properties_.end() );
+ // search for any children of this property, and delete them as well
+ deleteChildren( found_it->second );
+
+ delete found_it->second;
+
+ properties_.erase( found_it );
+}
+
+void PropertyManager::deleteChildren( PropertyBase* property )
+{
std::set<PropertyBase*> to_delete;
- // search for any children of this property, and delete them as well
+
M_Property::iterator prop_it = properties_.begin();
M_Property::iterator prop_end = properties_.end();
for ( ; prop_it != prop_end; ++prop_it )
{
- PropertyBase* property = prop_it->second;
+ PropertyBase* child = prop_it->second;
- if ( property->getParent() == found_it->second )
+ if ( child->getParent() == property )
{
- to_delete.insert( property );
+ to_delete.insert( child );
}
}
@@ -107,10 +117,6 @@
{
deleteProperty( *del_it );
}
-
- delete found_it->second;
-
- properties_.erase( found_it );
}
void PropertyManager::deleteByUserData( void* user_data )
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/properties/property_manager.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -115,6 +115,11 @@
* @param user_data The user data to compare against
*/
void deleteByUserData( void* user_data );
+ /**
+ * \brief Delete all the children of a property
+ * @param property The property whose children to delete
+ */
+ void deleteChildren( PropertyBase* property );
/**
* \brief Called when a property in the property grid is changing.
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -63,6 +63,7 @@
VisualizationManager::VisualizationManager( VisualizationPanel* panel )
: ogre_root_( Ogre::Root::getSingletonPtr() )
, vis_panel_( panel )
+, needs_reset_( false )
{
initializeCommon();
registerFactories( this );
@@ -111,10 +112,14 @@
setTargetFrame( "base" );
setFixedFrame( "map" );
+
+ ros_node_->subscribe( "/time", time_message_, &VisualizationManager::incomingROSTime, this, 1 );
}
VisualizationManager::~VisualizationManager()
{
+ ros_node_->unsubscribe( "/time", &VisualizationManager::incomingROSTime, this );
+
Disconnect( wxEVT_TIMER, update_timer_->GetId(), wxTimerEventHandler( VisualizationManager::onUpdate ), NULL, this );
delete update_timer_;
@@ -194,6 +199,13 @@
updateRelativeNode();
vis_panel_->getCurrentCamera()->update();
+
+ if ( needs_reset_ )
+ {
+ needs_reset_ = false;
+ resetVisualizers();
+ tf_->clear();
+ }
}
void VisualizationManager::addVisualizer( VisualizerBase* visualizer, bool allow_deletion, bool enabled )
@@ -223,6 +235,18 @@
vis_panel_->getPropertyGrid()->Refresh();
}
+void VisualizationManager::resetVisualizers()
+{
+ V_VisualizerInfo::iterator vis_it = visualizers_.begin();
+ V_VisualizerInfo::iterator vis_end = visualizers_.end();
+ for ( ; vis_it != vis_end; ++vis_it )
+ {
+ VisualizerBase* visualizer = vis_it->visualizer_;
+
+ visualizer->reset();
+ }
+}
+
inline void createParticle( Ogre::ParticleSystem* particle_sys, const Ogre::Vector3& position, float size )
{
Ogre::Particle* p = particle_sys->createParticle();
@@ -575,4 +599,16 @@
target_relative_node_->setOrientation( orientation );
}
+void VisualizationManager::incomingROSTime()
+{
+ static ros::Time last_time = ros::Time(0);
+
+ if ( time_message_.rostime < last_time )
+ {
+ needs_reset_ = true;
+ }
+
+ last_time = time_message_.rostime;
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -39,6 +39,8 @@
#include <vector>
#include <map>
+#include <rostools/Time.h>
+
namespace ogre_tools
{
class wxOgreRenderWindow;
@@ -201,6 +203,8 @@
Ogre::SceneNode* getTargetRelativeNode() { return target_relative_node_; }
+ void resetVisualizers();
+
protected:
/**
* \brief Add a visualizer to be managed by this panel
@@ -220,6 +224,8 @@
void updateRelativeNode();
+ void incomingROSTime();
+
Ogre::Root* ogre_root_; ///< Ogre Root
Ogre::SceneManager* scene_manager_; ///< Ogre scene manager associated with this panel
Ogre::RaySceneQuery* ray_scene_query_; ///< Used for querying the scene based on the mouse location
@@ -257,6 +263,9 @@
VisualizerSignal visualizer_state_;
Ogre::SceneNode* target_relative_node_;
+
+ rostools::Time time_message_;
+ bool needs_reset_;
};
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizer_base.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -158,6 +158,11 @@
*/
virtual const char* getType() = 0;
+ /**
+ * \brief Called to tell the visualizer to clear its state
+ */
+ virtual void reset() {}
+
protected:
/// Derived classes override this to do the actual work of enabling themselves
virtual void onEnable() = 0;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -173,11 +173,13 @@
{
RenderAutoLock renderLock( this );
- cloud_->setCloudVisible( false );
cloud_->clear();
points_.clear();
point_times_.clear();
cloud_messages_.clear();
+
+ intensity_min_ = 9999999.0f;
+ intensity_max_ = -9999999.0f;
}
void LaserScanVisualizer::onEnable()
@@ -191,6 +193,8 @@
unsubscribe();
clear();
+
+ cloud_->setCloudVisible( false );
}
void LaserScanVisualizer::subscribe()
@@ -302,8 +306,6 @@
float diff_intensity = intensity_max_ - intensity_min_;
-
-
points_.push_back( V_Point() );
V_Point& points = points_.back();
points.resize( point_count_ );
@@ -430,4 +432,9 @@
boost::bind( &LaserScanVisualizer::setCloudTopic, this, _1 ), parent_category_, this );
}
+void LaserScanVisualizer::reset()
+{
+ clear();
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -125,6 +125,7 @@
virtual void targetFrameChanged() {}
virtual void fixedFrameChanged();
virtual void createProperties();
+ virtual void reset();
static const char* getTypeStatic() { return "Laser Scan"; }
virtual const char* getType() { return getTypeStatic(); }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -251,6 +251,8 @@
}
loaded_ = true;
+
+ causeRender();
}
void MapVisualizer::transformMap()
@@ -311,4 +313,9 @@
transformMap();
}
+void MapVisualizer::reset()
+{
+ clear();
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -70,6 +70,7 @@
virtual void fixedFrameChanged();
virtual void createProperties();
virtual void update( float dt );
+ virtual void reset();
static const char* getTypeStatic() { return "Map"; }
virtual const char* getType() { return getTypeStatic(); }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -332,4 +332,9 @@
clearMarkers();
}
+void MarkerVisualizer::reset()
+{
+ clearMarkers();
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -106,6 +106,7 @@
virtual void targetFrameChanged() {}
virtual void fixedFrameChanged();
+ virtual void reset();
static const char* getTypeStatic() { return "Markers"; }
virtual const char* getType() { return getTypeStatic(); }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -252,4 +252,9 @@
boost::bind( &OctreeVisualizer::setOctreeTopic, this, _1 ), parent_category_, this );
}
+void OctreeVisualizer::reset()
+{
+ manual_object_->clear();
}
+
+}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -79,6 +79,7 @@
virtual void createProperties();
virtual void update( float dt );
+ virtual void reset();
static const char* getTypeStatic() { return "Octree"; }
virtual const char* getType() { return getTypeStatic(); }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -321,4 +321,11 @@
boost::bind( &PointCloudVisualizer::setTopic, this, _1 ), parent_category_, this );
}
+void PointCloudVisualizer::reset()
+{
+ RenderAutoLock renderLock( this );
+
+ cloud_->clear();
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -108,6 +108,7 @@
virtual void targetFrameChanged() {}
virtual void fixedFrameChanged();
virtual void createProperties();
+ virtual void reset();
static const char* getTypeStatic() { return "Point Cloud"; }
virtual const char* getType() { return getTypeStatic(); }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -49,7 +49,7 @@
: VisualizerBase( name, manager )
, topic_( "odom" )
, color_( 1.0f, 0.1f, 0.0f )
-, position_tolerance_( 0.01 )
+, position_tolerance_( 0.1 )
, angle_tolerance_( 0.1 )
, color_property_( NULL )
, topic_property_( NULL )
@@ -285,4 +285,9 @@
queue_mutex_.unlock();
}
+void RobotBase2DPoseVisualizer::reset()
+{
+ clear();
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -82,6 +82,7 @@
virtual void createProperties();
virtual void update( float dt );
virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return true; }
+ virtual void reset();
static const char* getTypeStatic() { return "RobotBase2DPose"; }
virtual const char* getType() { return getTypeStatic(); }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -255,5 +255,10 @@
robot_->setPropertyManager( property_manager_, parent_category_ );
}
+void RobotModelVisualizer::reset()
+{
+ has_new_transforms_ = true;
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -113,6 +113,7 @@
virtual void fixedFrameChanged() {}
virtual void createProperties();
virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return true; }
+ virtual void reset();
static const char* getTypeStatic() { return "Robot Model"; }
virtual const char* getType() { return getTypeStatic(); }
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.cpp 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.cpp 2008-10-29 18:29:17 UTC (rev 5925)
@@ -109,6 +109,8 @@
frames_.clear();
+ property_manager_->deleteChildren( tree_category_ );
+
update_timer_ = 0.0f;
}
@@ -289,11 +291,11 @@
try
{
- tf_->transformPose( target_frame_, pose, pose );
+ tf_->transformPose( fixed_frame_, pose, pose );
}
catch(tf::TransformException& e)
{
- ROS_ERROR( "Error transforming frame '%s' to frame '%s'\n", frame->name_.c_str(), target_frame_.c_str() );
+ ROS_ERROR( "Error transforming frame '%s' to frame '%s'\n", frame->name_.c_str(), fixed_frame_.c_str() );
}
frame->position_ = Ogre::Vector3( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
@@ -341,11 +343,11 @@
try
{
- tf_->transformPose( target_frame_, parent_pose, parent_pose );
+ tf_->transformPose( fixed_frame_, parent_pose, parent_pose );
}
catch(tf::TransformException& e)
{
- ROS_ERROR( "Error transforming frame '%s' to frame '%s'\n", frame->parent_.c_str(), target_frame_.c_str() );
+ ROS_ERROR( "Error transforming frame '%s' to frame '%s'\n", frame->parent_.c_str(), fixed_frame_.c_str() );
}
Ogre::Vector3 parent_position = Ogre::Vector3( parent_pose.getOrigin().x(), parent_pose.getOrigin().y(), parent_pose.getOrigin().z() );
@@ -440,5 +442,10 @@
update_timer_ = update_rate_;
}
+void TFVisualizer::reset()
+{
+ clear();
+}
+
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.h 2008-10-29 18:28:31 UTC (rev 5924)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/tf_visualizer.h 2008-10-29 18:29:17 UTC (rev 5925)
@@ -118,6 +118,7 @@
virtual void fixedFrameChanged() {}
virtual void createProperties();
virtual bool isObjectPickable( const Ogre::MovableObject* object ) const { return true; }
+ virtual void reset();
static const char* getTypeStatic() { return "TF"; }
virtual const char* getType() { return getTypeStatic(); }
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-10-29 18:34:00
|
Revision: 5926
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5926&view=rev
Author: jfaustwg
Date: 2008-10-29 18:33:53 +0000 (Wed, 29 Oct 2008)
Log Message:
-----------
r11046@lan-dhcp-121: jfaust | 2008-10-29 10:29:13 -0700
Fix orbit cam for the case where we have a relative node and yaw axis is changing
Modified Paths:
--------------
pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11000
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11046
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp
===================================================================
--- pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2008-10-29 18:29:17 UTC (rev 5925)
+++ pkg/trunk/visualization/ogre_tools/src/ogre_tools/orbit_camera.cpp 2008-10-29 18:33:53 UTC (rev 5926)
@@ -104,6 +104,8 @@
{
Ogre::Vector3 vec = pos - global_focal_point;
pos = relative_node_->getOrientation() * vec + global_focal_point;
+
+ camera_->setFixedYawAxis(true, relative_node_->getOrientation() * Ogre::Vector3::UNIT_Y);
}
camera_->setPosition( pos );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-10-29 18:34:28
|
Revision: 5927
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5927&view=rev
Author: jfaustwg
Date: 2008-10-29 18:34:25 +0000 (Wed, 29 Oct 2008)
Log Message:
-----------
r11053@lan-dhcp-121: jfaust | 2008-10-29 11:17:23 -0700
Add alpha to the map
Modified Paths:
--------------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11046
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11053
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-10-29 18:33:53 UTC (rev 5926)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp 2008-10-29 18:34:25 UTC (rev 5927)
@@ -58,7 +58,12 @@
, resolution_( 0.0f )
, width_( 0.0f )
, height_( 0.0f )
+, load_timer_( 2.0f )
, service_property_( NULL )
+, resolution_property_( NULL )
+, width_property_( NULL )
+, height_property_( NULL )
+, alpha_property_( NULL )
{
scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
@@ -70,6 +75,8 @@
material_->getTechnique(0)->setLightingEnabled(false);
material_->setDepthBias( -16.0f, 0.0f );
material_->setCullingMode( Ogre::CULL_NONE );
+
+ setAlpha( 0.7f );
}
MapVisualizer::~MapVisualizer()
@@ -87,6 +94,38 @@
clear();
}
+void MapVisualizer::setAlpha( float alpha )
+{
+ alpha_ = alpha;
+
+ Ogre::Pass* pass = material_->getTechnique(0)->getPass(0);
+ Ogre::TextureUnitState* tex_unit = NULL;
+ if (pass->getNumTextureUnitStates() > 0)
+ {
+ tex_unit = pass->getTextureUnitState(0);
+ }
+ else
+ {
+ tex_unit = pass->createTextureUnitState();
+ }
+
+ tex_unit->setAlphaOperation( Ogre::LBX_SOURCE1, Ogre::LBS_MANUAL, Ogre::LBS_CURRENT, alpha_ );
+
+ if ( alpha_ < 0.9998 )
+ {
+ material_->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
+ }
+ else
+ {
+ material_->setSceneBlending( Ogre::SBT_REPLACE );
+ }
+
+ if ( alpha_property_ )
+ {
+ alpha_property_->changed();
+ }
+}
+
void MapVisualizer::setService( const std::string& service )
{
service_ = service;
@@ -112,6 +151,7 @@
texture_.setNull();
Ogre::TextureManager::getSingleton().unload( tex_name );
+ load_timer_ = 2.0f;
loaded_ = false;
}
@@ -161,6 +201,7 @@
pixels[pidx+0] = val;
pixels[pidx+1] = val;
pixels[pidx+2] = val;
+ //pixels[pidx+3] = 1.0f;
}
}
@@ -281,16 +322,14 @@
void MapVisualizer::update( float dt )
{
- static float timer = 0.0f;
-
if ( !loaded_ )
{
- timer -= dt;
+ load_timer_ += dt;
- if ( timer < 0.0f )
+ if ( load_timer_ > 2.0f )
{
load();
- timer = 2.0f;
+ load_timer_ = 0.0f;
}
}
}
@@ -300,6 +339,9 @@
service_property_ = property_manager_->createProperty<StringProperty>( "Service", property_prefix_, boost::bind( &MapVisualizer::getService, this ),
boost::bind( &MapVisualizer::setService, this, _1 ), parent_category_, this );
+ alpha_property_ = property_manager_->createProperty<FloatProperty>( "Alpha", property_prefix_, boost::bind( &MapVisualizer::getAlpha, this ),
+ boost::bind( &MapVisualizer::setAlpha, this, _1 ), parent_category_, this );
+
resolution_property_ = property_manager_->createProperty<FloatProperty>( "Resolution", property_prefix_, boost::bind( &MapVisualizer::getResolution, this ),
FloatProperty::Setter(), parent_category_, this );
width_property_ = property_manager_->createProperty<FloatProperty>( "Width", property_prefix_, boost::bind( &MapVisualizer::getWidth, this ),
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h 2008-10-29 18:33:53 UTC (rev 5926)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h 2008-10-29 18:34:25 UTC (rev 5927)
@@ -65,6 +65,9 @@
float getWidth() { return width_; }
float getHeight() { return height_; }
+ float getAlpha() { return alpha_; }
+ void setAlpha( float alpha );
+
// Overrides from VisualizerBase
virtual void targetFrameChanged() {}
virtual void fixedFrameChanged();
@@ -95,10 +98,15 @@
float width_;
float height_;
+ float load_timer_;
+
+ float alpha_;
+
StringProperty* service_property_;
FloatProperty* resolution_property_;
FloatProperty* width_property_;
FloatProperty* height_property_;
+ FloatProperty* alpha_property_;
};
} // namespace ogre_vis
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-10-29 18:34:32
|
Revision: 5928
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5928&view=rev
Author: jfaustwg
Date: 2008-10-29 18:34:30 +0000 (Wed, 29 Oct 2008)
Log Message:
-----------
r11054@lan-dhcp-121: jfaust | 2008-10-29 11:19:03 -0700
Fix clear() to skip the first TimeCache, which is always NULL
Modified Paths:
--------------
pkg/trunk/util/tf/src/tf.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11053
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11054
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/util/tf/src/tf.cpp
===================================================================
--- pkg/trunk/util/tf/src/tf.cpp 2008-10-29 18:34:25 UTC (rev 5927)
+++ pkg/trunk/util/tf/src/tf.cpp 2008-10-29 18:34:30 UTC (rev 5928)
@@ -64,9 +64,12 @@
void Transformer::clear()
{
frame_mutex_.lock();
- for (std::vector< TimeCache*>::iterator cache_it = frames_.begin(); cache_it != frames_.end(); ++cache_it)
+ if ( frames_.size() > 1 )
{
- (*cache_it)->clearList();
+ for (std::vector< TimeCache*>::iterator cache_it = frames_.begin() + 1; cache_it != frames_.end(); ++cache_it)
+ {
+ (*cache_it)->clearList();
+ }
}
frame_mutex_.unlock();
}
@@ -95,10 +98,10 @@
//calculate first leg
TransformLists t_list = lookupLists(lookupFrameNumber( fixed_frame), source_time, lookupFrameNumber( source_frame));
btTransform temp1 = computeTransformFromList(t_list);
-
+
TransformLists t_list2 = lookupLists(lookupFrameNumber( target_frame), target_time, lookupFrameNumber( fixed_frame));
-
+
btTransform temp = computeTransformFromList(t_list2);
transform.setData( temp1 * temp); ///\todo check order here
@@ -109,10 +112,10 @@
bool Transformer::getParent(const std::string& frame_id, ros::Time time, std::string& parent)
{
-
- tf::TimeCache* cache = getFrame(lookupFrameNumber(frame_id));
+
+ tf::TimeCache* cache = getFrame(lookupFrameNumber(frame_id));
TransformStorage temp;
- try
+ try
{
cache->getData(time, temp);
}
@@ -120,7 +123,7 @@
{
return false;
}
-
+
parent = temp.parent_id_;
if (parent == "NO_PARENT")
return false;
@@ -140,8 +143,8 @@
///\todo add fixed frame support
TransformLists mTfLs;
- if (target_frame == source_frame)
- return mTfLs; //Don't do anythign if we're not going anywhere
+ if (target_frame == source_frame)
+ return mTfLs; //Don't do anythign if we're not going anywhere
TransformStorage temp;
@@ -225,14 +228,14 @@
if (mTfLs.forwardTransforms.size() == 0) //If it's going to itself it's already been caught
{
std::stringstream ss;
- ss<< "No Common ParentD between "<< lookupFrameString(target_frame) <<" and " << lookupFrameString(source_frame)
+ ss<< "No Common ParentD between "<< lookupFrameString(target_frame) <<" and " << lookupFrameString(source_frame)
<< std::endl << allFramesAsString() << std::endl;
throw(ConnectivityException(ss.str()));
}
if (lookupFrameNumber(mTfLs.forwardTransforms.back().frame_id_) != target_frame)
{
std::stringstream ss;
- ss<< "No Common ParentC between " << lookupFrameString(target_frame) <<" and " << lookupFrameString(source_frame)
+ ss<< "No Common ParentC between " << lookupFrameString(target_frame) <<" and " << lookupFrameString(source_frame)
<< std::endl << allFramesAsString() << std::endl << mTfLs.forwardTransforms.size() << " forward length" << std::endl;
throw(ConnectivityException(ss.str()));
}
@@ -458,32 +461,32 @@
};
-void Transformer::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
- const Stamped<Quaternion>& stamped_in,
- const std::string& fixed_frame,
+void Transformer::transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
+ const Stamped<Quaternion>& stamped_in,
+ const std::string& fixed_frame,
Stamped<Quaternion>& stamped_out)
{
Stamped<Transform> transform;
- lookupTransform(target_frame, target_time,
- stamped_in.frame_id_,stamped_in.stamp_,
+ lookupTransform(target_frame, target_time,
+ stamped_in.frame_id_,stamped_in.stamp_,
fixed_frame, transform);
-
+
stamped_out.setData( transform * stamped_in);
stamped_out.stamp_ = stamped_in.stamp_;
stamped_out.frame_id_ = target_frame;
};
-void Transformer::transformVector(const std::string& target_frame, const ros::Time& target_time,
- const Stamped<Vector3>& stamped_in,
- const std::string& fixed_frame,
+void Transformer::transformVector(const std::string& target_frame, const ros::Time& target_time,
+ const Stamped<Vector3>& stamped_in,
+ const std::string& fixed_frame,
Stamped<Vector3>& stamped_out)
{
Stamped<Transform> transform;
- lookupTransform(target_frame, target_time,
- stamped_in.frame_id_,stamped_in.stamp_,
+ lookupTransform(target_frame, target_time,
+ stamped_in.frame_id_,stamped_in.stamp_,
fixed_frame, transform);
-
+
/** \todo may not be most efficient */
btVector3 end = stamped_in;
btVector3 origin = btVector3(0,0,0);
@@ -495,14 +498,14 @@
};
-void Transformer::transformPoint(const std::string& target_frame, const ros::Time& target_time,
- const Stamped<Point>& stamped_in,
- const std::string& fixed_frame,
+void Transformer::transformPoint(const std::string& target_frame, const ros::Time& target_time,
+ const Stamped<Point>& stamped_in,
+ const std::string& fixed_frame,
Stamped<Point>& stamped_out)
{
Stamped<Transform> transform;
- lookupTransform(target_frame, target_time,
- stamped_in.frame_id_,stamped_in.stamp_,
+ lookupTransform(target_frame, target_time,
+ stamped_in.frame_id_,stamped_in.stamp_,
fixed_frame, transform);
stamped_out.setData(transform * stamped_in);
@@ -511,14 +514,14 @@
stamped_out.parent_id_ = stamped_in.parent_id_;//only useful for transforms
};
-void Transformer::transformPose(const std::string& target_frame, const ros::Time& target_time,
- const Stamped<Pose>& stamped_in,
- const std::string& fixed_frame,
+void Transformer::transformPose(const std::string& target_frame, const ros::Time& target_time,
+ const Stamped<Pose>& stamped_in,
+ const std::string& fixed_frame,
Stamped<Pose>& stamped_out)
{
Stamped<Transform> transform;
- lookupTransform(target_frame, target_time,
- stamped_in.frame_id_,stamped_in.stamp_,
+ lookupTransform(target_frame, target_time,
+ stamped_in.frame_id_,stamped_in.stamp_,
fixed_frame, transform);
stamped_out.setData(transform * stamped_in);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-10-30 01:15:54
|
Revision: 5981
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5981&view=rev
Author: rob_wheeler
Date: 2008-10-30 01:15:50 +0000 (Thu, 30 Oct 2008)
Log Message:
-----------
Support updated MCB firmware which fixes a bug in the calibration sensor reading.
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
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-30 01:09:28 UTC (rev 5980)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2008-10-30 01:15:50 UTC (rev 5981)
@@ -111,7 +111,7 @@
break;
case BEGINNING:
original_switch_state_ = actuator_->state_.calibration_reading_;
- cc_.steer_velocity_ = (original_switch_state_ ? search_velocity_ : -search_velocity_);
+ cc_.steer_velocity_ = (original_switch_state_ ? -search_velocity_ : search_velocity_);
state_ = MOVING;
break;
case MOVING: {
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-30 01:09:28 UTC (rev 5980)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-30 01:15:50 UTC (rev 5981)
@@ -123,7 +123,7 @@
break;
case BEGINNING:
original_switch_state_ = actuator_->state_.calibration_reading_;
- vc_.setCommand(original_switch_state_ ? search_velocity_ : -search_velocity_);
+ vc_.setCommand(original_switch_state_ ? -search_velocity_ : search_velocity_);
state_ = MOVING;
break;
case MOVING: {
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-30 01:09:28 UTC (rev 5980)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h 2008-10-30 01:15:50 UTC (rev 5981)
@@ -174,8 +174,8 @@
uint16_t num_encoder_errors_;
uint8_t encoder_status_;
uint8_t calibration_reading_;
+ int32_t last_calibration_rising_edge_;
int32_t last_calibration_falling_edge_;
- int32_t last_calibration_rising_edge_;
uint16_t board_temperature_;
uint16_t bridge_temperature_;
uint16_t supply_voltage_;
@@ -256,8 +256,10 @@
enum
{
- LIMIT_SENSOR_0_STATE = 1, LIMIT_SENSOR_1_STATE = 2,
- LIMIT_OFF_TO_ON = 4, LIMIT_ON_TO_OFF = 8
+ LIMIT_SENSOR_0_STATE = (1 << 0),
+ LIMIT_SENSOR_1_STATE = (1 << 1),
+ LIMIT_ON_TO_OFF = (1 << 2),
+ LIMIT_OFF_TO_ON = (1 << 3)
};
enum
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-30 01:09:28 UTC (rev 5980)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-30 01:15:50 UTC (rev 5981)
@@ -199,7 +199,7 @@
if (sh_->get_product_code() == WG05::PRODUCT_CODE)
{
- if (major != 1 || minor < 6)
+ if (major != 1 || minor != 7)
{
ROS_FATAL("Unsupported firmware revision %d.%02d\n", major, minor);
ROS_BREAK();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-10-30 20:53:18
|
Revision: 6032
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6032&view=rev
Author: isucan
Date: 2008-10-30 20:53:02 +0000 (Thu, 30 Oct 2008)
Log Message:
-----------
deprecated drawstuff package
Added Paths:
-----------
pkg/trunk/deprecated/drawstuff/
Removed Paths:
-------------
pkg/trunk/3rdparty/drawstuff/
Property changes on: pkg/trunk/deprecated/drawstuff
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-10-31 01:00:45
|
Revision: 6069
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6069&view=rev
Author: stuglaser
Date: 2008-10-31 01:00:39 +0000 (Fri, 31 Oct 2008)
Log Message:
-----------
The WristCalibrationController calibrates both joints of the wrist.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/wrist_transmission.h
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2008-10-31 00:57:26 UTC (rev 6068)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt 2008-10-31 01:00:39 UTC (rev 6069)
@@ -15,6 +15,7 @@
src/head_pan_tilt_controller.cpp
src/caster_controller.cpp
src/caster_calibration_controller.cpp
+ src/wrist_calibration_controller.cpp
)
rospack_add_executable(testBaseController test/test_base_controller.cpp)
Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/wrist_calibration_controller.h 2008-10-31 01:00:39 UTC (rev 6069)
@@ -0,0 +1,116 @@
+/*
+ * 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.
+ */
+
+/*
+ Sample config:
+ <controller type="WristCalibrationController" name="cal_wrist">
+ <calibrate transmission="wrist_trans"
+ actuator_l="wrist_l_motor" actuator_r="wrist_r_motor"
+ flex_joint="wrist_flex_joint" roll_joint="wrist_roll_joint"
+ velocity="0.6" />
+ <pid p="3.0" i="0.2" d="0" iClamp="2.0" />
+ </controller>
+
+ * Author: Stuart Glaser
+ */
+
+#ifndef WRIST_CALIBRATION_CONTROLLER_H
+#define WRIST_CALIBRATION_CONTROLLER_H
+
+#include "robot_mechanism_controllers/joint_velocity_controller.h"
+#include "misc_utils/realtime_publisher.h"
+#include "mechanism_model/wrist_transmission.h"
+#include "std_msgs/Empty.h"
+
+namespace controller {
+
+class WristCalibrationController : public Controller
+{
+public:
+ WristCalibrationController();
+ ~WristCalibrationController();
+
+ 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_ == CALIBRATED; }
+ void beginCalibration()
+ {
+ if (state_ == INITIALIZED)
+ state_ = BEGINNING;
+ }
+
+protected:
+
+ enum { INITIALIZED, BEGINNING, MOVING_FLEX, MOVING_ROLL, CALIBRATED };
+ int state_;
+
+ double search_velocity_;
+ bool original_switch_state_;
+
+ // Tracks the actuator positions for when the optical switch occurred.
+ double flex_switch_l_, flex_switch_r_;
+ double roll_switch_l_, roll_switch_r_;
+
+ double prev_actuator_l_position_, prev_actuator_r_position_;
+
+ Actuator *actuator_l_, *actuator_r_;
+ mechanism::JointState *flex_joint_, *roll_joint_;
+ mechanism::Transmission *transmission_;
+
+ controller::JointVelocityController vc_flex_, vc_roll_;
+};
+
+
+class WristCalibrationControllerNode : public Controller
+{
+public:
+ WristCalibrationControllerNode();
+ ~WristCalibrationControllerNode();
+
+ void update();
+
+ bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+private:
+ mechanism::RobotState *robot_;
+ WristCalibrationController c_;
+
+ double last_publish_time_;
+ misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
+};
+
+}
+
+#endif
Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/wrist_calibration_controller.cpp 2008-10-31 01:00:39 UTC (rev 6069)
@@ -0,0 +1,308 @@
+/*
+ * 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
+ */
+
+#include "pr2_mechanism_controllers/wrist_calibration_controller.h"
+
+namespace controller {
+
+WristCalibrationController::WristCalibrationController()
+ : state_(INITIALIZED)
+{
+}
+
+WristCalibrationController::~WristCalibrationController()
+{
+}
+
+bool WristCalibrationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ assert(config);
+
+ TiXmlElement *cal = config->FirstChildElement("calibrate");
+ if (!cal)
+ {
+ std::cerr<<"WristCalibrationController was not given calibration parameters"<<std::endl;
+ return false;
+ }
+
+ if(cal->QueryDoubleAttribute("velocity", &search_velocity_) != TIXML_SUCCESS)
+ {
+ std::cerr<<"Velocity value was not specified\n";
+ return false;
+ }
+
+ const char *flex_joint_name = cal->Attribute("flex_joint");
+ flex_joint_ = flex_joint_name ? robot->getJointState(flex_joint_name) : NULL;
+ if (!flex_joint_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find flex joint \"%s\"\n",
+ flex_joint_name);
+ return false;
+ }
+
+ const char *roll_joint_name = cal->Attribute("roll_joint");
+ roll_joint_ = roll_joint_name ? robot->getJointState(roll_joint_name) : NULL;
+ if (!roll_joint_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find roll_joint \"%s\"\n",
+ roll_joint_name);
+ return false;
+ }
+
+ const char *actuator_l_name = cal->Attribute("actuator_l");
+ actuator_l_ = actuator_l_name ? robot->model_->getActuator(actuator_l_name) : NULL;
+ if (!actuator_l_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find actuator_l \"%s\"\n",
+ actuator_l_name);
+ return false;
+ }
+
+ const char *actuator_r_name = cal->Attribute("actuator_r");
+ actuator_r_ = actuator_r_name ? robot->model_->getActuator(actuator_r_name) : NULL;
+ if (!actuator_r_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find actuator_r \"%s\"\n",
+ actuator_r_name);
+ return false;
+ }
+
+ const char *transmission_name = cal->Attribute("transmission");
+ transmission_ = transmission_name ? robot->model_->getTransmission(transmission_name) : NULL;
+ if (!transmission_)
+ {
+ fprintf(stderr, "Error: WristCalibrationController could not find transmission \"%s\"\n",
+ transmission_name);
+ return false;
+ }
+
+ control_toolbox::Pid pid;
+ TiXmlElement *p = config->FirstChildElement("pid");
+ if (p)
+ pid.initXml(p);
+ else
+ {
+ fprintf(stderr, "WristCalibrationController's config did not specify the default pid parameters.\n");
+ return false;
+ }
+
+ return
+ vc_flex_.init(robot, flex_joint_name, pid) &&
+ vc_roll_.init(robot, roll_joint_name, pid);
+}
+
+void WristCalibrationController::update()
+{
+ // Flex optical switch is connected to actuator_l
+ // Roll optical switch is connected to actuator_r
+
+ switch(state_)
+ {
+ case INITIALIZED:
+ vc_flex_.setCommand(0);
+ vc_roll_.setCommand(0);
+ state_ = BEGINNING;
+ break;
+ case BEGINNING:
+ original_switch_state_ = actuator_l_->state_.calibration_reading_;
+ vc_flex_.setCommand(original_switch_state_ ? -search_velocity_ : search_velocity_);
+ vc_roll_.setCommand(0);
+ state_ = MOVING_FLEX;
+ break;
+ case MOVING_FLEX: {
+ bool switch_state_ = actuator_l_->state_.calibration_reading_;
+ if (switch_state_ != original_switch_state_)
+ {
+ if (switch_state_ == true)
+ flex_switch_l_ = actuator_l_->state_.last_calibration_rising_edge_;
+ else
+ flex_switch_l_ = actuator_l_->state_.last_calibration_falling_edge_;
+
+ // But where was actuator_r at the transition? Unfortunately,
+ // actuator_r is not connected to the flex joint's optical
+ // switch, so we don't know directly. Instead, we estimate
+ // actuator_r's position based on the switch position of
+ // actuator_l.
+ double dl = actuator_l_->state_.position_ - prev_actuator_l_position_;
+ double dr = actuator_r_->state_.position_ - prev_actuator_r_position_;
+ double k = (flex_switch_l_ - prev_actuator_l_position_) / dl;
+ assert(0 <= k && k <= 1);
+ flex_switch_r_ = k * dr + prev_actuator_r_position_;
+
+ original_switch_state_ = actuator_r_->state_.calibration_reading_;
+ vc_flex_.setCommand(0);
+ vc_roll_.setCommand(original_switch_state_ ? -search_velocity_ : search_velocity_);
+ state_ = MOVING_ROLL;
+ }
+ break;
+ }
+ case MOVING_ROLL: {
+ bool switch_state_ = actuator_r_->state_.calibration_reading_;
+ if (switch_state_ != original_switch_state_)
+ {
+ if (switch_state_ == true)
+ flex_switch_r_ = actuator_r_->state_.last_calibration_rising_edge_;
+ else
+ flex_switch_r_ = actuator_r_->state_.last_calibration_falling_edge_;
+
+ // See corresponding comment above.
+ double dl = actuator_l_->state_.position_ - prev_actuator_l_position_;
+ double dr = actuator_r_->state_.position_ - prev_actuator_r_position_;
+ double k = (flex_switch_r_ - prev_actuator_r_position_) / dr;
+ assert(0 <= k && k <= 1);
+ flex_switch_l_ = k * dl + prev_actuator_l_position_;
+
+
+ //----------------------------------------------------------------------
+ // Calibration computation
+ //----------------------------------------------------------------------
+
+ // At this point, we know the actuator positions when the
+ // optical switches were hit. Now we compute the actuator
+ // positions when the joints should be at 0.
+
+ const int LEFT_MOTOR = mechanism::WristTransmission::LEFT_MOTOR;
+ const int RIGHT_MOTOR = mechanism::WristTransmission::RIGHT_MOTOR;
+ const int FLEX_JOINT = mechanism::WristTransmission::FLEX_JOINT;
+ const int ROLL_JOINT = mechanism::WristTransmission::ROLL_JOINT;
+
+ // Sets up the data structures for passing joint and actuator
+ // positions through the transmission.
+ Actuator fake_as_mem[2]; // This way we don't need to delete the objects later
+ mechanism::JointState fake_js_mem[2];
+ std::vector<Actuator*> fake_as;
+ std::vector<mechanism::JointState*> fake_js;
+ fake_as.push_back(&fake_as_mem[0]);
+ fake_as.push_back(&fake_as_mem[1]);
+ fake_js.push_back(&fake_js_mem[0]);
+ fake_js.push_back(&fake_js_mem[1]);
+
+ // Finds the (uncalibrated) joint position where the flex optical switch triggers
+ fake_as[LEFT_MOTOR]->state_.position_ = flex_switch_l_;
+ fake_as[RIGHT_MOTOR]->state_.position_ = flex_switch_r_;
+ transmission_->propagatePosition(fake_as, fake_js);
+ double flex_joint_switch_ = fake_js[FLEX_JOINT]->position_;
+
+ // Finds the (uncalibrated) joint position where the roll optical switch triggers
+ fake_as[LEFT_MOTOR]->state_.position_ = roll_switch_l_;
+ fake_as[RIGHT_MOTOR]->state_.position_ = roll_switch_r_;
+ transmission_->propagatePosition(fake_as, fake_js);
+ double roll_joint_switch_ = fake_js[ROLL_JOINT]->position_;
+
+ // Finds the (uncalibrated) joint position at the desired zero
+ fake_js[FLEX_JOINT]->position_ = flex_joint_switch_ - flex_joint_->joint_->reference_position_;
+ fake_js[ROLL_JOINT]->position_ = roll_joint_switch_ - roll_joint_->joint_->reference_position_;
+
+ // Determines the actuator zero position from the desired joint zero positions
+ transmission_->propagatePositionBackwards(fake_js, fake_as);
+ actuator_l_->state_.zero_offset_ = fake_as[LEFT_MOTOR]->state_.position_;
+ actuator_r_->state_.zero_offset_ = fake_as[RIGHT_MOTOR]->state_.position_;
+
+ flex_joint_->calibrated_ = true;
+ roll_joint_->calibrated_ = true;
+ state_ = CALIBRATED;
+
+ vc_flex_.setCommand(0);
+ vc_roll_.setCommand(0);
+ }
+
+ break;
+ }
+ case CALIBRATED:
+ break;
+ }
+
+ if (state_ != CALIBRATED)
+ {
+ vc_flex_.update();
+ vc_roll_.update();
+ }
+
+ prev_actuator_l_position_ = actuator_l_->state_.position_;
+ prev_actuator_r_position_ = actuator_r_->state_.position_;
+}
+
+
+ROS_REGISTER_CONTROLLER(WristCalibrationControllerNode)
+
+WristCalibrationControllerNode::WristCalibrationControllerNode()
+: last_publish_time_(0), pub_calibrated_(NULL)
+{
+}
+
+WristCalibrationControllerNode::~WristCalibrationControllerNode()
+{
+ if (pub_calibrated_)
+ delete pub_calibrated_;
+}
+
+void WristCalibrationControllerNode::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 WristCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+ assert(robot);
+ robot_ = robot;
+
+ std::string name = config->Attribute("name") ? config->Attribute("name") : "";
+ if (name == "")
+ {
+ fprintf(stderr, "No name given to WristCalibrationController\n");
+ return false;
+ }
+
+ if (!c_.initXml(robot, config))
+ return false;
+
+ pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(name + "/calibrated", 1);
+
+ return true;
+}
+
+}
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/wrist_transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/wrist_transmission.h 2008-10-31 00:57:26 UTC (rev 6068)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/wrist_transmission.h 2008-10-31 01:00:39 UTC (rev 6069)
@@ -33,15 +33,15 @@
*********************************************************************/
/*
* Author: Melonee Wise
-
+
example xml:
<robot name="wrist_trans">
- <joint name="right_wrist_flex_joint" type="revolute">
+ <joint name="right_wrist_flex_joint" type="revolute">
<limit min="-0.157" max="2.409" effort="5" velocity="5" />
<axis xyz="0 0 1" />
</joint>
- <joint name="right_wrist_roll_joint" type="continuous">
+ <joint name="right_wrist_roll_joint" type="continuous">
<limit min="0.0" max="0.0" effort="5" velocity="5" />
<axis xyz="0 0 1" />
</joint>
@@ -74,6 +74,11 @@
std::vector<double> mechanical_reduction_;
+ // Describes the order of the actuators and the joints in the arrays
+ // of names and of those passed to propagate*
+ enum { RIGHT_MOTOR, LEFT_MOTOR };
+ enum { FLEX_JOINT, ROLL_JOINT };
+
void propagatePosition(std::vector<Actuator*>&, std::vector<JointState*>&);
void propagatePositionBackwards(std::vector<JointState*>&, std::vector<Actuator*>&);
void propagateEffort(std::vector<JointState*>&, std::vector<Actuator*>&);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-10-31 19:57:30
|
Revision: 6105
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6105&view=rev
Author: hsujohnhsu
Date: 2008-10-31 19:57:23 +0000 (Fri, 31 Oct 2008)
Log Message:
-----------
added sim for prototype1, this runs near realtime.
Modified Paths:
--------------
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_gazebo_prototype1.xml
Added Paths:
-----------
pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.xml
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_headless.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1_headless.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_head_tilt_laser_torso_gazebo.xml
Added: pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.xml (rev 0)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-gazebo-prototype1-wg-fake_localization.xml 2008-10-31 19:57:23 UTC (rev 6105)
@@ -0,0 +1,19 @@
+<launch>
+ <master auto="start"/>
+ <group name="wg">
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1.launch"/>
+ <include file="$(find 2dnav_gazebo)/2dnav-params.xml"/>
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
+ <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
+ <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+
+ <!-- for visualization -->
+ <!--node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" /-->
+
+ <!-- for manual control -->
+ <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ <!--node pkg="teleop_arm_keyboard" type="teleop_arm_keyboard" respawn="false" output="screen" /-->
+ </group>
+</launch>
+
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-10-31 19:57:19 UTC (rev 6104)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-10-31 19:57:23 UTC (rev 6105)
@@ -5,7 +5,6 @@
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_prototype1/gazebo/pr2_prototype1.xml"" />
-
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_prototype1.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-10-31 19:57:19 UTC (rev 6104)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-10-31 19:57:23 UTC (rev 6105)
@@ -2,7 +2,7 @@
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
<param name="base_controller/odom_publish_rate" value="10" />
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_base_lab.xml" output="screen"/>
- <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/gazebo/controllers_head_tilt_laser_torso.xml" output="screen"/>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/gazebo/controllers_head_tilt_laser_torso_gazebo.xml" output="screen"/>
<!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
for details of the profile, rates, see controller::LaserScannerControllerNode. -->
<node pkg="robot_mechanism_controllers" type="control.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
Added: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_headless.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_headless.launch (rev 0)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_headless.launch 2008-10-31 19:57:23 UTC (rev 6105)
@@ -0,0 +1,19 @@
+<launch>
+ <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
+ <!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
+ <group name="wg">
+ <!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
+ (Mechanism Control, BaseControllerNode, etc...) -->
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_prototype1/gazebo/pr2_prototype1.xml"" />
+ <!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
+ <node pkg="gazebo" type="gazebo" args="-g -n $(find gazebo_robot_description)/world/robot_prototype1_headless.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ <!-- use mech.py to spawn all controllers listed in controllers.xml -->
+ <include file="$(find pr2_prototype1_gazebo)/pr2_prototype1_controllers.launch" />
+ </group>
+</launch>
+
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-10-31 19:57:19 UTC (rev 6104)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-10-31 19:57:23 UTC (rev 6105)
@@ -39,14 +39,6 @@
<type>fltk</type>
<size>1024 800</size>
<pos>0 0</pos>
- <frames>
- <row height="100%">
- <camera width="100%">
- <xyz>0 0 20</xyz>
- <rpy>0 90 90</rpy>
- </camera>
- </row>
- </frames>
</rendering:gui>
@@ -60,7 +52,6 @@
<maxUpdateRate>100</maxUpdateRate>
</rendering:ogre>
-
<model:physical name="gplane">
<xyz>0 0 0</xyz>
<rpy>0 0 0</rpy>
@@ -72,254 +63,41 @@
<kd>1.0</kd>
<normal>0 0 1</normal>
<size>51.3 51.3</size>
- <!-- map3.png -->
- <material>PR2/floor_texture</material>
+ <material>Gazebo/GrassFloor</material>
</geom:plane>
</body:plane>
</model:physical>
- <!--
- <model:empty name="ros_model">
- <body:empty name="ros_body">
- <controller:ros_node name="ros_node" plugin="libRos_Node.so">
- <nodeName>simulator_ros_node</nodeName>
- </controller:ros_node>
- </body:empty>
- </model:empty>
- -->
-
- <!-- The "desk"-->
- <model:physical name="desk1_model">
- <xyz> 2.28 -0.21 -0.10</xyz>
- <rpy> 0.0 0.0 0.00</rpy>
- <body:box name="desk1_legs_body">
- <geom:box name="desk1_legs_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <xyz> 0.0 0.0 0.50</xyz>
- <mesh>default</mesh>
- <size>0.5 1.0 0.75</size>
- <mass> 10.0</mass>
- <visual>
- <size> 0.5 1.0 0.75</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- <geom:box name="desk1_top_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <xyz> 0.0 0.0 0.90</xyz>
- <mesh>default</mesh>
- <size>0.75 1.5 0.05</size>
- <mass> 10.0</mass>
- <visual>
- <size> 0.75 1.5 0.05</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
+<!--
+ <model:physical name="walls">
+ <include embedded="false">
+ <xi:include href="tests/willow-walls.model" />
+ </include>
</model:physical>
+-->
+ <model:physical name="willow_map">
+ <xyz>-25.65 25.65 1.0</xyz>
+ <rpy>180 0 0</rpy>
+ <static>true</static>
+ <body:map name="willow_map_body">
+ <geom:map name="willow_map_geom">
+ <image>willowMap.png</image>
+ <threshold>200</threshold>
+ <granularity>2</granularity>
+ <negative>false</negative>
+ <scale>0.1</scale>
+ <offset>0 0 0</offset>
+ <material>Gazebo/Rocky</material>
+ </geom:map>
+ </body:map>
+ </model:physical>
- <!-- The second "desk"-->
- <model:physical name="desk2_model">
- <xyz> 2.25 -3.0 -0.10</xyz>
- <rpy> 0.0 0.0 0.00</rpy>
- <body:box name="desk2_legs_body">
- <geom:box name="desk2_legs_geom">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <xyz> 0.0 0.0 0.50</xyz>
- <mesh>default</mesh>
- <size>0.5 1.0 0.75</size>
- <mass> 10.0</mass>
- <visual>
- <size> 0.5 1.0 0.75</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- <geom:box name="desk2_top_geom">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <xyz> 0.0 0.0 0.90</xyz>
- <mesh>default</mesh>
- <size>0.75 1.5 0.05</size>
- <mass> 10.0</mass>
- <visual>
- <size> 0.75 1.5 0.05</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
- </model:physical>
- <!-- The small cylinder "cup" -->
- <model:physical name="cylinder1_model">
- <xyz> 2.5 0.0 0.9</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <body:cylinder name="cylinder1_body">
- <geom:cylinder name="cylinder1_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <size>0.025 0.075</size>
- <mass> 0.05</mass>
- <visual>
- <size> 0.05 0.05 0.075</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_cylinder</mesh>
- </visual>
- </geom:cylinder>
- <geom:box name="cylinder1_base_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <xyz>0.0 0.0 -0.033</xyz>
- <size>0.05 0.05 0.01</size>
- <mass> 0.01</mass>
- <visual>
- <size> 0.05 0.05 0.01</size>
- <material>Gazebo/Fish</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:cylinder>
- </model:physical>
-
- <!-- The small box "cup" -->
- <model:physical name="object1_model">
- <xyz> 0.835 -0.55 0.95</xyz>
- <rpy> 0.0 0.0 30.0</rpy>
- <body:box name="object1_body">
- <geom:box name="object1_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <size>0.1 0.03 0.03</size>
- <mass> 0.05</mass>
- <visual>
- <size> 0.1 0.030 0.03</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
-
- <controller:P3D name="p3d_object_controller" plugin="libP3D.so">
- <updateRate>100.0</updateRate>
- <bodyName>object1_body</bodyName>
- <topicName>object1_body_ground_truth</topicName>
- <frameName>object1_body_ground_truth_frame</frameName>
- <interface:position name="p3d_object_position"/>
- </controller:P3D>
-
- </model:physical>
-
-
- <!-- The small ball -->
- <model:physical name="sphere1_model">
- <xyz> 2.5 -2.8 1.0</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <body:sphere name="sphere1_body">
- <geom:sphere name="sphere1_geom">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <mesh>default</mesh>
- <size> 0.15</size>
- <mass> 1.0</mass>
- <visual>
- <size> 0.3 0.3 0.3</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_sphere</mesh>
- </visual>
- </geom:sphere>
- </body:sphere>
- </model:physical>
-
- <!-- The large ball map3.png -->
- <model:physical name="sphere2_model">
- <xyz> 5.85 4.35 1.55</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <body:sphere name="sphere2_body">
- <geom:sphere name="sphere2_geom">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <mesh>default</mesh>
- <size> 1.0</size>
- <mass> 1.0</mass>
- <visual>
- <size> 2.0 2.0 2.0</size>
- <material>Gazebo/Rocky</material>
- <mesh>unit_sphere</mesh>
- </visual>
- </geom:sphere>
- </body:sphere>
- </model:physical>
-
- <!-- The wall in front map3.png -->
- <model:physical name="wall_2_model">
- <xyz> 11.6 -1.55 1.0</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <static>true</static>
- <body:box name="wall_2_body">
- <geom:box name="wall_2_geom">
- <mesh>default</mesh>
- <size>2.1 32.8 2.0</size>
- <visual>
- <size>2.1 32.8 2.0</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
- </model:physical>
-
- <!-- The wall behind -->
- <model:physical name="wall_1_model">
- <xyz> -11.3 -1.45 1.0</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <static>true</static>
- <body:box name="wall_1_body">
- <geom:box name="wall_1_geom">
- <mesh>default</mesh>
- <size>0.4 24.0 2.0</size>
- <visual>
- <size>0.4 24.0 2.0</size>
- <material>Gazebo/PioneerBody</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
- </model:physical>
-
- <!-- The wall 3 -->
- <model:physical name="wall_3_model">
- <xyz> 6.7 8.05 1.0</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <static>true</static>
- <body:box name="wall_3_body">
- <geom:box name="wall_3_geom">
- <mesh>default</mesh>
- <size>7.5 1.2 2.0</size>
- <visual>
- <size>7.5 1.2 2.0</size>
- <material>Gazebo/Chrome</material>
- <mesh>unit_box</mesh>
- </visual>
- </geom:box>
- </body:box>
- </model:physical>
-
-
-
<model:physical name="robot_model1">
<controller:ros_time name="ros_time" plugin="libRos_Time.so">
<alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
+ <updateRate>1000.0</updateRate>
<interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
</controller:ros_time>
@@ -335,6 +113,7 @@
<!-- White Directional light -->
+ <!--
<model:renderable name="directional_white">
<light>
<type>directional</type>
@@ -344,6 +123,7 @@
<attenuation>1 0.0 1.0 0.4</attenuation>
</light>
</model:renderable>
+ -->
</gazebo:world>
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1_headless.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1_headless.world (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1_headless.world 2008-10-31 19:57:23 UTC (rev 6105)
@@ -0,0 +1,129 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
+
+ <verbosity>5</verbosity>
+
+<!-- cfm is 1e-5 for single precision -->
+<!-- erp is typically .1-.8 -->
+<!-- here's the global contact cfm/erp -->
+ <physics:ode>
+ <stepTime>0.001</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.0000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>true</quickStep>
+ </physics:ode>
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <size>1024 800</size>
+ <pos>0 0</pos>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>false</grid>
+ <maxUpdateRate>100</maxUpdateRate>
+ </rendering:ogre>
+
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <material>Gazebo/GrassFloor</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+<!--
+ <model:physical name="walls">
+ <include embedded="false">
+ <xi:include href="tests/willow-walls.model" />
+ </include>
+ </model:physical>
+-->
+ <model:physical name="willow_map">
+ <xyz>-25.65 25.65 1.0</xyz>
+ <rpy>180 0 0</rpy>
+ <static>true</static>
+ <body:map name="willow_map_body">
+ <geom:map name="willow_map_geom">
+ <image>willowMap.png</image>
+ <threshold>200</threshold>
+ <granularity>2</granularity>
+ <negative>false</negative>
+ <scale>0.1</scale>
+ <offset>0 0 0</offset>
+ <material>Gazebo/Rocky</material>
+ </geom:map>
+ </body:map>
+ </model:physical>
+
+
+ <model:physical name="robot_model1">
+
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
+ <xyz>0.0 0.0 0.0408</xyz> <!-- bottom of base off the ground by this much, basically wheel height below skirt -->
+ <rpy>0.0 0.0 0.0 </rpy>
+
+ <!-- base, torso and arms -->
+ <include embedded="true">
+ <xi:include href="pr2_xml_prototype1.model" />
+ </include>
+
+ </model:physical>
+
+
+ <!-- White Directional light -->
+ <!--
+ <model:renderable name="directional_white">
+ <light>
+ <type>directional</type>
+ <direction>0 -0.5 -0.5</direction>
+ <diffuseColor>0.4 0.4 0.4</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
+ <attenuation>1 0.0 1.0 0.4</attenuation>
+ </light>
+ </model:renderable>
+ -->
+
+
+</gazebo:world>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_gazebo_prototype1.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_gazebo_prototype1.xml 2008-10-31 19:57:19 UTC (rev 6104)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_gazebo_prototype1.xml 2008-10-31 19:57:23 UTC (rev 6105)
@@ -10,7 +10,7 @@
<!-- PR2_ACTARRAY -->
<controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
<alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
+ <updateRate>1000.0</updateRate>
<gazebo_physics filename="gazebo_joints_prototype1.xml" /> <!-- for simulator/physics specific settigs -->
@@ -38,6 +38,17 @@
<interface:audio name="battery_dummy_interface" />
</controller:gazebo_battery>
+ <controller:P3D name="p3d_base_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bodyName>base</bodyName>
+ <topicName>base_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
+ <rpyOffsets>0 0 0</rpyOffsets>
+ <interface:position name="p3d_base_position"/>
+ </controller:P3D>
+
</verbatim>
</map>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_head_tilt_laser_torso_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_head_tilt_laser_torso_gazebo.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/gazebo/controllers_head_tilt_laser_torso_gazebo.xml 2008-10-31 19:57:23 UTC (rev 6105)
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+
+<controllers>
+
+
+ <!-- ========================================= -->
+ <!-- torso array -->
+ <controller name="torso_controller" topic="torso_controller" type="JointPositionControllerNode">
+ <joint name="torso_joint">
+ <pid p="1000" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <!-- ========================================= -->
+ <!-- head and above array -->
+ <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
+ <listen_topic name="head_pan_commands" />
+ <joint name="head_pan_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
+ <listen_topic name="head_tilt_commands" />
+ <joint name="head_tilt_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+
+ <controller name="tilt_laser_controller" topic="tilt_laser_controller" type="LaserScannerControllerNode">
+ <joint name="tilt_laser_mount_joint" >
+ <pid p="0.4" d="0" i="0" iClamp="0.1" />
+ </joint>
+ </controller>
+
+</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-31 22:24:34
|
Revision: 6118
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6118&view=rev
Author: gerkey
Date: 2008-10-31 22:24:24 +0000 (Fri, 31 Oct 2008)
Log Message:
-----------
Bumped player rev, to bring in reversion of bad change.
Update amcl_player to use transforms and synthesize odom messages.
Modified Paths:
--------------
pkg/trunk/3rdparty/player/Makefile
pkg/trunk/nav/amcl_player/amcl_player.cc
Modified: pkg/trunk/3rdparty/player/Makefile
===================================================================
--- pkg/trunk/3rdparty/player/Makefile 2008-10-31 22:12:59 UTC (rev 6117)
+++ pkg/trunk/3rdparty/player/Makefile 2008-10-31 22:24:24 UTC (rev 6118)
@@ -2,7 +2,7 @@
SVN_DIR = player-svn
SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/player/trunk
-SVN_REVISION = -r 7108
+SVN_REVISION = -r 7123
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-10-31 22:12:59 UTC (rev 6117)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-10-31 22:24:24 UTC (rev 6118)
@@ -82,25 +82,29 @@
@todo Expose the various amcl parameters via ROS.
**/
-#include <assert.h>
+#include <deque>
+
+#include "rosconsole/rosassert.h"
+
// For core Player stuff (message queues, config file objects, etc.)
-#include <libplayercore/playercore.h>
+#include "libplayercore/playercore.h"
// TODO: remove XDR dependency
-#include <libplayerxdr/playerxdr.h>
+#include "libplayerxdr/playerxdr.h"
// roscpp
-#include <ros/node.h>
+#include "ros/node.h"
// Messages that I need
-#include <std_msgs/LaserScan.h>
-#include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/ParticleCloud2D.h>
-#include <std_msgs/Pose2DFloat32.h>
-#include <std_srvs/StaticMap.h>
+#include "std_msgs/LaserScan.h"
+#include "std_msgs/RobotBase2DOdom.h"
+#include "std_msgs/ParticleCloud2D.h"
+#include "std_msgs/Pose2DFloat32.h"
+#include "std_srvs/StaticMap.h"
// For transform support
-#include <tf/transform_broadcaster.h>
+#include "tf/transform_broadcaster.h"
+#include "tf/transform_listener.h"
// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
@@ -133,7 +137,9 @@
int setPose(double x, double y, double a);
private:
- tf::TransformBroadcaster* tf;
+ tf::TransformBroadcaster* tf;
+ tf::TransformListener* tfL;
+
ConfigFile* cf;
// incoming messages
@@ -172,6 +178,12 @@
ros::Duration cloud_pub_interval;
ros::Time last_cloud_pub_time;
+
+ // Helper to get odometric pose from transform system
+ bool getOdomPose(double& x, double& y, double& yaw, const ros::Time& t);
+
+ // buffer of not-yet-transformed scans
+ std::deque<std_msgs::LaserScan> laser_scans;
};
#define USAGE "USAGE: amcl_player"
@@ -214,9 +226,15 @@
// TODO: remove XDR dependency
playerxdr_ftable_init();
+ this->tf = new tf::TransformBroadcaster(*this);
+ this->tfL = new tf::TransformListener(*this, true,
+ 10000000000ULL,
+ 200000000ULL);
+
+
advertise<std_msgs::RobotBase2DOdom>("localizedpose",2);
advertise<std_msgs::ParticleCloud2D>("particlecloud",2);
- subscribe("odom", odomMsg, &AmclNode::odomReceived,2);
+ //subscribe("odom", odomMsg, &AmclNode::odomReceived,2);
subscribe("scan", laserMsg, &AmclNode::laserReceived,2);
subscribe("initialpose", initialPoseMsg, &AmclNode::initialPoseReceived,2);
@@ -255,7 +273,7 @@
/// way of retrieving such Txs;
//
// retrieve static laser transform, if it's available
- param("laser_x_offset", laser_x_offset, 0.05);
+ param("laser_x_offset", laser_x_offset, 0.0);
//assert(read_map_from_image(&this->sx, &this->sy, &this->mapdata, fname, 0)
//== 0);
@@ -299,19 +317,19 @@
this->position2d_dev = deviceTable->AddDevice(this->position2d_addr,
NULL, false);
- assert(this->position2d_dev);
+ ROS_ASSERT(this->position2d_dev);
this->position2d_dev->InQueue = QueuePointer(this->Driver::InQueue);
this->position2d_dev->driver = (Driver*)this;
this->laser_dev = deviceTable->AddDevice(this->laser_addr,
NULL, false);
- assert(this->laser_dev);
+ ROS_ASSERT(this->laser_dev);
this->laser_dev->InQueue = QueuePointer(this->Driver::InQueue);
this->laser_dev->driver = (Driver*)this;
this->map_dev = deviceTable->AddDevice(this->map_addr,
NULL, false);
- assert(this->map_dev);
+ ROS_ASSERT(this->map_dev);
this->map_dev->InQueue = QueuePointer(this->Driver::InQueue);
this->map_dev->driver = (Driver*)this;
@@ -334,7 +352,7 @@
// Turn this on for serious AMCL debugging, but you must have built
// player with ENABLE_RTKGUI=ON.
- //this->cf->InsertFieldValue(0,"enable_gui","1");
+ this->cf->InsertFieldValue(0,"enable_gui","1");
// Grab params off the param server
int max_beams, min_samples, max_samples;
@@ -396,7 +414,7 @@
// Create an instance of the driver, passing it the ConfigFile object.
// The -1 tells it to look into the "global" section of the ConfigFile,
// which is where ConfigFile::InsertFieldValue() put the parameters.
- assert((this->driver = AdaptiveMCL_Init(cf, -1)));
+ ROS_ASSERT((this->driver = AdaptiveMCL_Init(cf, -1)));
// Print out warnings about parameters that were set, but which the
// driver never looked at.
@@ -404,11 +422,9 @@
// Grab from the global deviceTable a pointer to the Device that was
// created as part of the driver's initialization.
- assert((this->pdevice = deviceTable->GetDevice(oposition2d_saddr,false)));
- assert((this->ldevice = deviceTable->GetDevice(olocalize_addr,false)));
+ ROS_ASSERT((this->pdevice = deviceTable->GetDevice(oposition2d_saddr,false)));
+ ROS_ASSERT((this->ldevice = deviceTable->GetDevice(olocalize_addr,false)));
- this->tf = new tf::TransformBroadcaster(*this);
-
double startX, startY, startTH;
param("robot_x_start", startX, 0.0);
param("robot_y_start", startY, 0.0);
@@ -444,9 +460,14 @@
// publish new transform robot->map
ros::Time t;
t.fromSec(hdr->timestamp);
+ /*
this->tf->sendTransform(tf::Stamped<tf::Transform> (tf::Transform(tf::Quaternion(pdata->pos.pa, 0, 0),
- tf::Point(pdata->pos.px, pdata->pos.py, 0.0)),
- t, "base","map"));
+ tf::Point(pdata->pos.px, pdata->pos.py, 0.0)),
+ t, "base","map"));
+ */
+ this->tf->sendTransform(tf::Stamped<tf::Transform> (tf::Transform(tf::Quaternion(pdata->pos.pa, 0, 0),
+ tf::Point(pdata->pos.px, pdata->pos.py, 0.0)).inverse(),
+ t, "map", "base"));
/*
printf("lpose: (%.3f %.3f %.3f) @ (%llu:%llu)\n",
@@ -471,6 +492,13 @@
// WTF is this?
printf("Somehow could not set frame_id to map\n");
}
+ /*
+ printf("O: %.6f %.3f %.3f %.3f\n",
+ hdr->timestamp,
+ localizedOdomMsg.pos.x,
+ localizedOdomMsg.pos.y,
+ localizedOdomMsg.pos.th);
+ */
publish("localizedpose", localizedOdomMsg);
if((ros::Time::now() - this->last_cloud_pub_time) >= cloud_pub_interval)
@@ -542,7 +570,7 @@
sj = mapresp.height = mapreq->height;
mapresp.data_count = mapresp.width * mapresp.height;
mapresp.data = new int8_t [mapresp.data_count];
- assert(mapresp.data);
+ ROS_ASSERT(mapresp.data);
// Grab the pixels from the map
for(j = 0; j < sj; j++)
{
@@ -671,81 +699,109 @@
return(0);
}
-void
-AmclNode::laserReceived()
+bool
+AmclNode::getOdomPose(double& x, double& y, double& yaw, const ros::Time& t)
{
- // Where was the robot when this scan was taken?
- /*
- libTF::TFPose2D robotPose;
- robotPose.x = robotPose.y = robotPose.yaw = 0.0;
- robotPose.frame = "base";
- robotPose.time = laserMsg.header.stamp.toNSec();
- libTF::TFPose2D odomPose;
+ // Get the robot's pose
+ tf::Stamped<tf::Pose> ident (btTransform(btQuaternion(0,0,0),
+ btVector3(0,0,0)), t, "base_laser");
+ tf::Stamped<btTransform> odom_pose;
try
{
- odomPose = this->tfClient.transformPose2D("odom", robotPose);
+ this->tfL->transformPose("odom", ident, odom_pose);
}
- catch(...)
+ catch(tf::TransformException e)
{
- puts("exception");
+ //ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
+ return false;
}
+ x = odom_pose.getOrigin().x();
+ y = odom_pose.getOrigin().y();
+ double pitch,roll;
+ odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);
- printf("pose: %.3f %.3f %.3f\n",
- odomPose.x,
- odomPose.y,
- odomPose.yaw);
- */
+ return true;
+}
+void
+AmclNode::laserReceived()
+{
+ // Put it on the queue
+ std_msgs::LaserScan newscan(laserMsg);
+ laser_scans.push_back(newscan);
- // Got new scan; reformat and pass it on
- player_laser_data_t pdata;
- pdata.min_angle = this->laserMsg.angle_min;
- pdata.max_angle = this->laserMsg.angle_max;
- pdata.resolution = this->laserMsg.angle_increment;
- // HACK, until the hokuyourg_player node is fixed
- if(this->laserMsg.range_max > 0.1)
- pdata.max_range = this->laserMsg.range_max;
- else
- pdata.max_range = 30.0;
- pdata.ranges_count = this->laserMsg.get_ranges_size();
- pdata.ranges = new float[pdata.ranges_count];
- assert(pdata.ranges);
- // We have to iterate over, rather than block copy, the ranges, because
- // we have to filter out short readings.
- for(unsigned int i=0;i<pdata.ranges_count;i++)
+ // Process the queued scans
+ while(!laser_scans.empty())
{
- // Player doesn't have a concept of min range. So we'll map short
- // readings to max range.
- if(this->laserMsg.ranges[i] <= this->laserMsg.range_min)
- pdata.ranges[i] = this->laserMsg.range_max;
+ std_msgs::LaserScan scan = laser_scans.front();
+
+ // Where was the robot when this scan was taken?
+ double x, y, yaw;
+ if(!getOdomPose(x, y, yaw, scan.header.stamp))
+ break;
+
+ laser_scans.pop_front();
+
+ double timestamp = scan.header.stamp.to_double();
+ //printf("I: %.6f %.3f %.3f %.3f\n",
+ //timestamp, x, y, yaw);
+
+ // Synthesize an odometry message
+ player_position2d_data_t pdata_odom;
+ pdata_odom.pos.px = x;
+ pdata_odom.pos.py = y;
+ pdata_odom.pos.pa = yaw;
+ pdata_odom.vel.px = 0.0;
+ pdata_odom.vel.py = 0.0;
+ pdata_odom.vel.pa = 0.0;
+ pdata_odom.stall = 0;
+
+ this->Driver::Publish(this->position2d_addr,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_POSITION2D_DATA_STATE,
+ (void*)&pdata_odom,0,
+ ×tamp);
+
+
+ // Got new scan; reformat and pass it on
+ player_laser_data_t pdata;
+ pdata.min_angle = scan.angle_min;
+ pdata.max_angle = scan.angle_max;
+ pdata.resolution = scan.angle_increment;
+ // HACK, until the hokuyourg_player node is fixed
+ if(scan.range_max > 0.1)
+ pdata.max_range = scan.range_max;
else
- pdata.ranges[i] = this->laserMsg.ranges[i];
- }
- pdata.intensity_count = this->laserMsg.get_intensities_size();
- pdata.intensity = new uint8_t[pdata.intensity_count];
- assert(pdata.intensity);
- /*
- for(unsigned int i=0;i<pdata.intensity_count;i++)
- {
- // AMCL doesn't care about intensity data
- //pdata.intensity[i] = this->laserMsg.intensities[i];
- pdata.intensity[i] = 0;
- }
- */
- ///\todo Optimize from above (not working at right)
- memset(pdata.intensity,0,sizeof(uint8_t)*pdata.intensity_count);
- pdata.id = this->laserMsg.header.seq;
+ pdata.max_range = 30.0;
+ pdata.ranges_count = scan.get_ranges_size();
+ pdata.ranges = new float[pdata.ranges_count];
+ ROS_ASSERT(pdata.ranges);
+ // We have to iterate over, rather than block copy, the ranges, because
+ // we have to filter out short readings.
+ for(unsigned int i=0;i<pdata.ranges_count;i++)
+ {
+ // Player doesn't have a concept of min range. So we'll map short
+ // readings to max range.
+ if(scan.ranges[i] <= scan.range_min)
+ pdata.ranges[i] = scan.range_max;
+ else
+ pdata.ranges[i] = scan.ranges[i];
+ }
+ pdata.intensity_count = scan.get_intensities_size();
+ pdata.intensity = new uint8_t[pdata.intensity_count];
+ ROS_ASSERT(pdata.intensity);
+ memset(pdata.intensity,0,sizeof(uint8_t)*pdata.intensity_count);
+ pdata.id = scan.header.seq;
- double timestamp = this->laserMsg.header.stamp.to_double();
+ this->Driver::Publish(this->laser_addr,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_LASER_DATA_SCAN,
+ (void*)&pdata,0,
+ ×tamp);
- this->Driver::Publish(this->laser_addr,
- PLAYER_MSGTYPE_DATA,
- PLAYER_LASER_DATA_SCAN,
- (void*)&pdata,0,
- ×tamp);
-
- delete[] pdata.ranges;
- delete[] pdata.intensity;
+ delete[] pdata.ranges;
+ delete[] pdata.intensity;
+ }
}
void
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2008-11-04 00:12:44
|
Revision: 6182
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6182&view=rev
Author: jfaustwg
Date: 2008-11-04 00:12:37 +0000 (Tue, 04 Nov 2008)
Log Message:
-----------
Merge "nav viewified" visualizer from user branch
r11450@iad: jfaust | 2008-10-31 15:41:28 -0700
First pass at "tools" in the visualizer -- includes the goal/pose tools from nav_view
r11511@iad: jfaust | 2008-11-03 15:42:39 -0800
Added automatic map reloading based on metadata subscription
Also in this checkin:
Forgot to checkin a number of source files
Modified Paths:
--------------
pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
pkg/trunk/visualization/ogre_visualizer/VisualizationPanel.fbp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/generated/visualization_panel_generated.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/generated/visualization_panel_generated.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_manager.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualization_panel.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/map_visualizer.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/poly_line_2d_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/poly_line_2d_visualizer.h
pkg/trunk/world_models/map_server/src/main.cpp
Added Paths:
-----------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/move_tool.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/move_tool.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/pose_tool.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/pose_tool.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/tool.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/tool.h
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11054
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11511
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt 2008-11-04 00:06:37 UTC (rev 6181)
+++ pkg/trunk/visualization/ogre_visualizer/CMakeLists.txt 2008-11-04 00:12:37 UTC (rev 6182)
@@ -2,8 +2,8 @@
include(rosbuild)
include(FindPkgConfig)
-set(ROS_BUILD_TYPE RelWithDebInfo)
-#set(ROS_BUILD_TYPE Debug)
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+set(ROS_BUILD_TYPE Debug)
rospack(ogre_visualizer)
@@ -61,6 +61,9 @@
rospack_add_library(${PROJECT_NAME} src/ogre_visualizer/properties/property.cpp
src/ogre_visualizer/properties/property_manager.cpp
+ src/ogre_visualizer/tools/tool.cpp
+ src/ogre_visualizer/tools/move_tool.cpp
+ src/ogre_visualizer/tools/pose_tool.cpp
src/ogre_visualizer/visualization_panel.cpp
src/ogre_visualizer/visualization_manager.cpp
src/ogre_visualizer/visualizer_base.cpp
Modified: pkg/trunk/visualization/ogre_visualizer/VisualizationPanel.fbp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/VisualizationPanel.fbp 2008-11-04 00:06:37 UTC (rev 6181)
+++ pkg/trunk/visualization/ogre_visualizer/VisualizationPanel.fbp 2008-11-04 00:12:37 UTC (rev 6182)
@@ -444,56 +444,118 @@
<property name="orient">wxVERTICAL</property>
<property name="permission">protected</property>
<object class="sizeritem" expanded="1">
- <property name="border">0</property>
+ <property name="border">5</property>
<property name="flag">wxEXPAND</property>
<property name="proportion">0</property>
- <object class="wxToolBar" expanded="1">
- <property name="bg"></property>
- <property name="bitmapsize"></property>
- <property name="context_help"></property>
- <property name="enabled">1</property>
- <property name="fg"></property>
- <property name="font"></property>
- <property name="hidden">0</property>
- <property name="id">wxID_ANY</property>
- <property name="margins"></property>
- <property name="maximum_size"></property>
+ <object class="wxBoxSizer" expanded="1">
<property name="minimum_size"></property>
- <property name="name">views_</property>
- <property name="packing">1</property>
- <property name="permission">protected</property>
- <property name="pos"></property>
- <property name="separation">5</property>
- <property name="size"></property>
- <property name="style">wxTB_HORIZONTAL|wxTB_NOICONS|wxTB_TEXT</property>
- <property name="subclass"></property>
- <property name="tooltip"></property>
- <property name="window_extra_style"></property>
- <property name="window_name"></property>
- <property name="window_style"></property>
- <event name="OnChar"></event>
- <event name="OnEnterWindow"></event>
- <event name="OnEraseBackground"></event>
- <event name="OnKeyDown"></event>
- <event name="OnKeyUp"></event>
- <event name="OnKillFocus"></event>
- <event name="OnLeaveWindow"></event>
- <event name="OnLeftDClick"></event>
- <event name="OnLeftDown"></event>
- <event name="OnLeftUp"></event>
- <event name="OnMiddleDClick"></event>
- <event name="OnMiddleDown"></event>
- <event name="OnMiddleUp"></event>
- <event name="OnMotion"></event>
- <event name="OnMouseEvents"></event>
- <event name="OnMouseWheel"></event>
- <event name="OnPaint"></event>
- <event name="OnRightDClick"></event>
- <event name="OnRightDown"></event>
- <event name="OnRightUp"></event>
- <event name="OnSetFocus"></event>
- <event name="OnSize"></event>
- <event name="OnUpdateUI"></event>
+ <property name="name">bSizer9</property>
+ <property name="orient">wxHORIZONTAL</property>
+ <property name="permission">none</property>
+ <object class="sizeritem" expanded="1">
+ <property name="border">5</property>
+ <property name="flag">wxALL</property>
+ <property name="proportion">0</property>
+ <object class="wxChoice" expanded="1">
+ <property name="bg"></property>
+ <property name="choices"></property>
+ <property name="context_help"></property>
+ <property name="enabled">1</property>
+ <property name="fg"></property>
+ <property name="font"></property>
+ <property name="hidden">0</property>
+ <property name="id">wxID_ANY</property>
+ <property name="maximum_size"></property>
+ <property name="minimum_size">150,-1</property>
+ <property name="name">views_</property>
+ <property name="permission">protected</property>
+ <property name="pos"></property>
+ <property name="selection">0</property>
+ <property name="size"></property>
+ <property name="subclass"></property>
+ <property name="tooltip"></property>
+ <property name="window_extra_style"></property>
+ <property name="window_name"></property>
+ <property name="window_style"></property>
+ <event name="OnChar"></event>
+ <event name="OnChoice">onViewSelected</event>
+ <event name="OnEnterWindow"></event>
+ <event name="OnEraseBackground"></event>
+ <event name="OnKeyDown"></event>
+ <event name="OnKeyUp"></event>
+ <event name="OnKillFocus"></event>
+ <event name="OnLeaveWindow"></event>
+ <event name="OnLeftDClick"></event>
+ <event name="OnLeftDown"></event>
+ <event name="OnLeftUp"></event>
+ <event name="OnMiddleDClick"></event>
+ <event name="OnMiddleDown"></event>
+ <event name="OnMiddleUp"></event>
+ <event name="OnMotion"></event>
+ <event name="OnMouseEvents"></event>
+ <event name="OnMouseWheel"></event>
+ <event name="OnPaint"></event>
+ <event name="OnRightDClick"></event>
+ <event name="OnRightDown"></event>
+ <event name="OnRightUp"></event>
+ <event name="OnSetFocus"></event>
+ <event name="OnSize"></event>
+ <event name="OnUpdateUI"></event>
+ </object>
+ </object>
+ <object class="sizeritem" expanded="1">
+ <property name="border">0</property>
+ <property name="flag">wxEXPAND</property>
+ <property name="proportion">1</property>
+ <object class="wxToolBar" expanded="1">
+ <property name="bg"></property>
+ <property name="bitmapsize"></property>
+ <property name="context_help"></property>
+ <property name="enabled">1</property>
+ <property name="fg"></property>
+ <property name="font"></property>
+ <property name="hidden">0</property>
+ <property name="id">wxID_ANY</property>
+ <property name="margins"></property>
+ <property name="maximum_size"></property>
+ <property name="minimum_size"></property>
+ <property name="name">tools_</property>
+ <property name="packing">1</property>
+ <property name="permission">protected</property>
+ <property name="pos"></property>
+ <property name="separation">5</property>
+ <property name="size"></property>
+ <property name="style">wxTB_HORIZONTAL|wxTB_NOICONS|wxTB_TEXT</property>
+ <property name="subclass"></property>
+ <property name="tooltip"></property>
+ <property name="window_extra_style"></property>
+ <property name="window_name"></property>
+ <property name="window_style"></property>
+ <event name="OnChar"></event>
+ <event name="OnEnterWindow"></event>
+ <event name="OnEraseBackground"></event>
+ <event name="OnKeyDown"></event>
+ <event name="OnKeyUp"></event>
+ <event name="OnKillFocus"></event>
+ <event name="OnLeaveWindow"></event>
+ <event name="OnLeftDClick"></event>
+ <event name="OnLeftDown"></event>
+ <event name="OnLeftUp"></event>
+ <event name="OnMiddleDClick"></event>
+ <event name="OnMiddleDown"></event>
+ <event name="OnMiddleUp"></event>
+ <event name="OnMotion"></event>
+ <event name="OnMouseEvents"></event>
+ <event name="OnMouseWheel"></event>
+ <event name="OnPaint"></event>
+ <event name="OnRightDClick"></event>
+ <event name="OnRightDown"></event>
+ <event name="OnRightUp"></event>
+ <event name="OnSetFocus"></event>
+ <event name="OnSize"></event>
+ <event name="OnUpdateUI"></event>
+ </object>
+ </object>
</object>
</object>
</object>
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/generated/visualization_panel_generated.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/generated/visualization_panel_generated.cpp 2008-11-04 00:06:37 UTC (rev 6181)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/generated/visualization_panel_generated.cpp 2008-11-04 00:12:37 UTC (rev 6182)
@@ -64,11 +64,23 @@
render_panel_ = new wxPanel( main_splitter_, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL );
render_sizer_ = new wxBoxSizer( wxVERTICAL );
- views_ = new wxToolBar( render_panel_, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxTB_HORIZONTAL|wxTB_NOICONS|wxTB_TEXT );
- views_->Realize();
+ wxBoxSizer* bSizer9;
+ bSizer9 = new wxBoxSizer( wxHORIZONTAL );
- render_sizer_->Add( views_, 0, wxEXPAND, 0 );
+ wxArrayString views_Choices;
+ views_ = new wxChoice( render_panel_, wxID_ANY, wxDefaultPosition, wxDefaultSize, views_Choices, 0 );
+ views_->SetSelection( 0 );
+ views_->SetMinSize( wxSize( 150,-1 ) );
+ bSizer9->Add( views_, 0, wxALL, 5 );
+
+ tools_ = new wxToolBar( render_panel_, wxID_ANY, wxDefaultPosition, wxDefaultSize, wxTB_HORIZONTAL|wxTB_NOICONS|wxTB_TEXT );
+ tools_->Realize();
+
+ bSizer9->Add( tools_, 1, wxEXPAND, 0 );
+
+ render_sizer_->Add( bSizer9, 0, wxEXPAND, 5 );
+
render_panel_->SetSizer( render_sizer_ );
render_panel_->Layout();
render_sizer_->Fit( render_panel_ );
@@ -81,6 +93,7 @@
// Connect Events
new_display_->Connect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VisualizationPanelGenerated::onNewDisplay ), NULL, this );
delete_display_->Connect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VisualizationPanelGenerated::onDeleteDisplay ), NULL, this );
+ views_->Connect( wxEVT_COMMAND_CHOICE_SELECTED, wxCommandEventHandler( VisualizationPanelGenerated::onViewSelected ), NULL, this );
}
VisualizationPanelGenerated::~VisualizationPanelGenerated()
@@ -88,6 +101,7 @@
// Disconnect Events
new_display_->Disconnect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VisualizationPanelGenerated::onNewDisplay ), NULL, this );
delete_display_->Disconnect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( VisualizationPanelGenerated::onDeleteDisplay ), NULL, this );
+ views_->Disconnect( wxEVT_COMMAND_CHOICE_SELECTED, wxCommandEventHandler( VisualizationPanelGenerated::onViewSelected ), NULL, this );
}
NewDisplayDialogGenerated::NewDisplayDialogGenerated( wxWindow* parent, wxWindowID id, const wxString& title, const wxPoint& pos, const wxSize& size, long style ) : wxDialog( parent, id, title, pos, size, style )
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/generated/visualization_panel_generated.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/generated/visualization_panel_generated.h 2008-11-04 00:06:37 UTC (rev 6181)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/generated/visualization_panel_generated.h 2008-11-04 00:12:37 UTC (rev 6182)
@@ -16,6 +16,7 @@
#include <wx/settings.h>
#include <wx/string.h>
#include <wx/button.h>
+#include <wx/choice.h>
#include <wx/toolbar.h>
#include <wx/splitter.h>
#include <wx/listbox.h>
@@ -43,11 +44,13 @@
wxButton* delete_display_;
wxPanel* render_panel_;
wxBoxSizer* render_sizer_;
- wxToolBar* views_;
+ wxChoice* views_;
+ wxToolBar* tools_;
// Virtual event handlers, overide them in your derived class
virtual void onNewDisplay( wxCommandEvent& event ){ event.Skip(); }
virtual void onDeleteDisplay( wxCommandEvent& event ){ event.Skip(); }
+ virtual void onViewSelected( wxCommandEvent& event ){ event.Skip(); }
public:
Added: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/move_tool.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/move_tool.cpp (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/move_tool.cpp 2008-11-04 00:12:37 UTC (rev 6182)
@@ -0,0 +1,126 @@
+/*
+ * 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.
+ */
+
+#include "move_tool.h"
+#include "visualization_manager.h"
+#include "visualization_panel.h"
+#include "ogre_tools/camera_base.h"
+
+#include <wx/wx.h>
+
+namespace ogre_vis
+{
+
+MoveTool::MoveTool( const std::string& name, VisualizationManager* manager )
+: Tool( name, manager )
+{
+
+}
+
+int MoveTool::processMouseEvent( wxMouseEvent& event, int last_x, int last_y )
+{
+ int flags = 0;
+
+ ogre_tools::CameraBase* camera = manager_->getVisualizationPanel()->getCurrentCamera();
+
+ if ( event.LeftDown() )
+ {
+ if ( event.ControlDown() )
+ {
+ manager_->pick( event.GetX(), event.GetY() );
+ }
+ else
+ {
+ camera->mouseLeftDown( event.GetX(), event.GetY() );
+ }
+
+ flags |= Render;
+ }
+ else if ( event.MiddleDown() )
+ {
+ camera->mouseMiddleDown( event.GetX(), event.GetY() );
+ flags |= Render;
+ }
+ else if ( event.RightDown() )
+ {
+ camera->mouseRightDown( event.GetX(), event.GetY() );
+ flags |= Render;
+ }
+ else if ( event.LeftUp() )
+ {
+ camera->mouseLeftUp( event.GetX(), event.GetY() );
+ flags |= Render;
+ }
+ else if ( event.MiddleUp() )
+ {
+ camera->mouseMiddleUp( event.GetX(), event.GetY() );
+ flags |= Render;
+ }
+ else if ( event.RightUp() )
+ {
+ camera->mouseRightUp( event.GetX(), event.GetY() );
+ flags |= Render;
+ }
+ else if ( event.Dragging() )
+ {
+ int32_t diff_x = event.GetX() - last_x;
+ int32_t diff_y = event.GetY() - last_y;
+
+ if ( event.LeftIsDown() )
+ {
+ camera->mouseLeftDrag( diff_x, diff_y );
+
+ flags |= Render;
+ }
+ else if ( event.MiddleIsDown() )
+ {
+ camera->mouseMiddleDrag( diff_x, diff_y );
+
+ flags |= Render;
+ }
+ else if ( event.RightIsDown() )
+ {
+ camera->mouseRightDrag( diff_x, diff_y );
+
+ flags |= Render;
+ }
+ }
+
+ if ( event.GetWheelRotation() != 0 )
+ {
+ camera->scrollWheel( event.GetWheelRotation() );
+
+ flags |= Render;
+ }
+
+ return flags;
+}
+
+}
+
Added: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/move_tool.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/move_tool.h (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/move_tool.h 2008-11-04 00:12:37 UTC (rev 6182)
@@ -0,0 +1,54 @@
+/*
+ * 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.
+ */
+
+#ifndef OGRE_VISUALIZER_MOVE_TOOL_H
+#define OGRE_VISUALIZER_MOVE_TOOL_H
+
+#include "tool.h"
+
+namespace ogre_vis
+{
+
+class VisualizationManager;
+
+class MoveTool : public Tool
+{
+public:
+ MoveTool( const std::string& name, VisualizationManager* manager );
+
+ virtual void activate() {}
+ virtual void deactivate() {}
+
+ virtual int processMouseEvent( wxMouseEvent& event, int last_x, int last_y );
+};
+
+}
+
+#endif
+
Added: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/pose_tool.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/pose_tool.cpp (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/pose_tool.cpp 2008-11-04 00:12:37 UTC (rev 6182)
@@ -0,0 +1,174 @@
+/*
+ * 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 COPYRIG...
[truncated message content] |
|
From: <ge...@us...> - 2008-11-04 08:49:52
|
Revision: 6232
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6232&view=rev
Author: gerkey
Date: 2008-11-04 08:49:46 +0000 (Tue, 04 Nov 2008)
Log Message:
-----------
Turned off extrapolation in amcl.
Fixed busy loop in startup of Player amcl driver.
Upped obstacle threshold on Stage's loading of maps.
Modified Paths:
--------------
pkg/trunk/3rdparty/player/Makefile
pkg/trunk/3rdparty/stage/Makefile
pkg/trunk/nav/amcl_player/amcl_player.cc
Modified: pkg/trunk/3rdparty/player/Makefile
===================================================================
--- pkg/trunk/3rdparty/player/Makefile 2008-11-04 08:17:37 UTC (rev 6231)
+++ pkg/trunk/3rdparty/player/Makefile 2008-11-04 08:49:46 UTC (rev 6232)
@@ -2,7 +2,7 @@
SVN_DIR = player-svn
SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/player/trunk
-SVN_REVISION = -r 7123
+SVN_REVISION = -r 7125
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/3rdparty/stage/Makefile
===================================================================
--- pkg/trunk/3rdparty/stage/Makefile 2008-11-04 08:17:37 UTC (rev 6231)
+++ pkg/trunk/3rdparty/stage/Makefile 2008-11-04 08:49:46 UTC (rev 6232)
@@ -2,7 +2,7 @@
SVN_DIR = stage-svn
SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/stage/branches/stage-ros
-SVN_REVISION = -r 7118
+SVN_REVISION = -r 7124
include $(shell rospack find mk)/svn_checkout.mk
build: SVN_UP stage
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-11-04 08:17:37 UTC (rev 6231)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-11-04 08:49:46 UTC (rev 6232)
@@ -426,13 +426,10 @@
// Give Player a chance to start up, under heavy load conditions.
// TODO: remove this.
- usleep(5000000);
+ //usleep(5000000);
this->tf = new tf::TransformBroadcaster(*this);
- this->tfL = new tf::TransformListener(*this, true,
- 10000000000ULL,
- 200000000ULL);
-
+ this->tfL = new tf::TransformListener(*this);
advertise<std_msgs::RobotBase2DOdom>("localizedpose",2);
advertise<std_msgs::ParticleCloud2D>("particlecloud",2);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|