You can subscribe to this list here.
2008 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
(4) |
Dec
|
---|---|---|---|---|---|---|---|---|---|---|---|---|
2009 |
Jan
|
Feb
|
Mar
(7) |
Apr
|
May
|
Jun
(40) |
Jul
(28) |
Aug
(51) |
Sep
(63) |
Oct
(42) |
Nov
(47) |
Dec
(19) |
2010 |
Jan
(5) |
Feb
(12) |
Mar
(23) |
Apr
(102) |
May
(2) |
Jun
(11) |
Jul
(32) |
Aug
(51) |
Sep
(64) |
Oct
(40) |
Nov
(86) |
Dec
(29) |
2011 |
Jan
(1) |
Feb
(10) |
Mar
(5) |
Apr
|
May
(3) |
Jun
(5) |
Jul
(3) |
Aug
(2) |
Sep
(11) |
Oct
(5) |
Nov
|
Dec
(1) |
2012 |
Jan
(6) |
Feb
(8) |
Mar
(2) |
Apr
(2) |
May
|
Jun
(2) |
Jul
(3) |
Aug
(2) |
Sep
|
Oct
|
Nov
(1) |
Dec
|
From: <tum...@li...> - 2012-11-26 04:17:52
|
Revision: 1012 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1012&view=rev Author: moesenle Date: 2012-11-26 04:17:45 +0000 (Mon, 26 Nov 2012) Log Message: ----------- Removed perception/objects_of_daily_use_finder git-commit Removed Paths: ------------- perception/objects_of_daily_use_finder/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-08-27 22:20:46
|
Revision: 1010 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1010&view=rev Author: moesenle Date: 2012-08-27 22:20:40 +0000 (Mon, 27 Aug 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@gm...> Date: Mon Aug 27 15:13:41 2012 -0700 commit 54c8ada27e640e1be245d07c62dbcd0d0d47db8b Author: Dejan Pangercic <dej...@gm...> Date: Mon Aug 27 15:13:00 2012 -0700 bugfix: corrupted launch file commit 3f1472995cecee2c6e568289333743ef43b15608 Author: Dejan Pangercic <dej...@gm...> Date: Mon Aug 27 15:02:14 2012 -0700 public export git-commit Modified Paths: -------------- perception/objects_of_daily_use_finder/launch/recognize.launch Modified: perception/objects_of_daily_use_finder/launch/recognize.launch =================================================================== --- perception/objects_of_daily_use_finder/launch/recognize.launch 2012-08-27 22:05:07 UTC (rev 1009) +++ perception/objects_of_daily_use_finder/launch/recognize.launch 2012-08-27 22:20:40 UTC (rev 1010) @@ -1,44 +1,33 @@ <launch> - <node pkg="objects_of_daily_use_finder" type="odu_finder_node" name="odu_finder_node" output="screen"> - <!-- launch-prefix="xterm -e gdb -args" --> - <param name="command" type="string" value="/load" /> - - <param name="database_location" type="string" value="/u/pangerci/spring_demo_2011db/database_kinect2/data" /> - <param name="images_folder" type="string" value="/u/pangerci/spring_demo_2011db/database_kinect2/images" /> - <param name="images_for_visualization_directory" type="string" value="/u/pangerci/spring_demo_2011db/database_kinect2/data" /> - - <param name="extract_roi" type="bool" value="true" /> - <param name="image_topic" type="string" value="/pointcloud_to_image_projector_opencv_node/image_with_projected_cluster" /> - - <node pkg="objects_of_daily_use_finder" type="odu_finder_node" name="odu_finder_node" output="screen" > - <!-- launch-prefix="xterm -e gdb -args" --> - <param name="command" type="string" value="/load" /> - - <param name="database_location" type="string" value="/home/monica/database_kinect/data/" /> - <param name="images_folder" type="string" value="/home/monica/database_kinect/images/" /> - <param name="images_for_visualization_directory" type="string" value="/home/monica/database_kinect/data/" /> - - <param name="extract_roi" type="bool" value="true" /> - <!-- <param name="image_topic" type="string" value="/narrow_stereo/left/image_rect" /> --> - <param name="image_topic" type="string" value="/camera/rgb/image_mono" /> - - <param name="template_topic" type="string" value="/object_id" /> - - <param name="enable_clustering" type="int" value="1" /> - <param name="enable_incremental_learning" type="int" value="0" /> - <param name="enable_visualization" type="int" value="1" /> - - <param name="votes_count" type="int" value="10" /> - - <param name="tree_k" type="int" value="6" /> - <param name="tree_levels" type="int" value="6" /> - - <param name="unknown_object_threshold" type="double" value="0.0" /> - - <param name="radius_adaptation_r_min" type="double" value="200" /> - <param name="radius_adaptation_r_mAX" type="double" value="600" /> - <param name="radius_adaptation_K" type="double" value="0.02" /> - <param name="radius_adaptation_A" type="double" value="800" /> - <param name="templates_to_show" type="int" value="4" /> - </node> + <node pkg="objects_of_daily_use_finder" type="odu_finder_node" name="odu_finder_node" output="screen" > + <!-- launch-prefix="xterm -e gdb -args" --> + <param name="command" type="string" value="/load" /> + + <param name="database_location" type="string" value="/home/monica/database_kinect/data/" /> + <param name="images_folder" type="string" value="/home/monica/database_kinect/images/" /> + <param name="images_for_visualization_directory" type="string" value="/home/monica/database_kinect/data/" /> + + <param name="extract_roi" type="bool" value="true" /> + <!-- <param name="image_topic" type="string" value="/narrow_stereo/left/image_rect" /> --> + <param name="image_topic" type="string" value="/camera/rgb/image_mono" /> + + <param name="template_topic" type="string" value="/object_id" /> + + <param name="enable_clustering" type="int" value="1" /> + <param name="enable_incremental_learning" type="int" value="0" /> + <param name="enable_visualization" type="int" value="1" /> + + <param name="votes_count" type="int" value="10" /> + + <param name="tree_k" type="int" value="6" /> + <param name="tree_levels" type="int" value="6" /> + + <param name="unknown_object_threshold" type="double" value="0.0" /> + + <param name="radius_adaptation_r_min" type="double" value="200" /> + <param name="radius_adaptation_r_mAX" type="double" value="600" /> + <param name="radius_adaptation_K" type="double" value="0.02" /> + <param name="radius_adaptation_A" type="double" value="800" /> + <param name="templates_to_show" type="int" value="4" /> + </node> </launch> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-08-07 14:37:07
|
Revision: 1007 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1007&view=rev Author: moesenle Date: 2012-08-07 14:36:57 +0000 (Tue, 07 Aug 2012) Log Message: ----------- Author: Ingo Kresse <kr...@in...> Date: Tue Aug 7 16:36:08 2012 +0200 publishing multi-joint velocity controller git-commit Modified Paths: -------------- controllers/ias_mechanism_controllers/CMakeLists.txt controllers/ias_mechanism_controllers/controller_plugins.xml controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/rosie_odometry.h controllers/ias_mechanism_controllers/manifest.xml controllers/ias_mechanism_controllers/src/rosie_odometry.cpp Added Paths: ----------- controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h controllers/ias_mechanism_controllers/launch/ controllers/ias_mechanism_controllers/launch/pr2_loopback.launch controllers/ias_mechanism_controllers/launch/pr2_multi_vel.yaml controllers/ias_mechanism_controllers/launch/rosie_loopback.launch controllers/ias_mechanism_controllers/launch/rosie_multi_vel.yaml controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp Modified: controllers/ias_mechanism_controllers/CMakeLists.txt =================================================================== --- controllers/ias_mechanism_controllers/CMakeLists.txt 2012-07-18 11:05:18 UTC (rev 1006) +++ controllers/ias_mechanism_controllers/CMakeLists.txt 2012-08-07 14:36:57 UTC (rev 1007) @@ -10,6 +10,9 @@ #rosbuild_genmsg() #gensrv() -rosbuild_add_library(ias_mechanism_controllers src/rosie_odometry.cpp) +rosbuild_add_library(ias_mechanism_controllers src/rosie_odometry.cpp + src/multi_joint_velocity_controller.cpp) rosbuild_add_boost_directories() rosbuild_link_boost(ias_mechanism_controllers thread) + + Modified: controllers/ias_mechanism_controllers/controller_plugins.xml =================================================================== --- controllers/ias_mechanism_controllers/controller_plugins.xml 2012-07-18 11:05:18 UTC (rev 1006) +++ controllers/ias_mechanism_controllers/controller_plugins.xml 2012-08-07 14:36:57 UTC (rev 1007) @@ -1,3 +1,4 @@ <library path="lib/libias_mechanism_controllers"> - <class name="RosieOdometry" type="controller::RosieOdometry" base_class_type="pr2_controller_interface::Controller" /> + <class name="ias_mechanism_controllers/RosieOdometry" type="controller::RosieOdometry" base_class_type="pr2_controller_interface::Controller" /> + <class name="ias_mechanism_controllers/MultiJointVelocityController" type="controller::MultiJointVelocityController" base_class_type="pr2_controller_interface::Controller" /> </library> Added: controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h =================================================================== --- controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h (rev 0) +++ controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h 2012-08-07 14:36:57 UTC (rev 1007) @@ -0,0 +1,98 @@ +/* + * 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 + + \class pr2_controller_interface::JointTrajectoryActionController + +*/ + +#ifndef MULTI_JOINT_VELOCITY_CONTROLLER_H__ +#define MULTI_JOINT_VELOCITY_CONTROLLER_H__ + +#include <vector> + +#include <boost/scoped_ptr.hpp> +#include <boost/thread/recursive_mutex.hpp> +#include <boost/thread/condition.hpp> +#include <ros/node_handle.h> + +#include <control_toolbox/pid.h> +#include <filters/filter_chain.h> +#include <pr2_controller_interface/controller.h> +#include <realtime_tools/realtime_publisher.h> + +#include <std_msgs/Float64MultiArray.h> +#include <pr2_controllers_msgs/JointTrajectoryControllerState.h> + + +namespace controller { + + +class MultiJointVelocityController : public pr2_controller_interface::Controller +{ +public: + + MultiJointVelocityController(); + ~MultiJointVelocityController(); + + bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); + + void starting(); + void update(); + +private: + int loop_count_; + pr2_mechanism_model::RobotState *robot_; + ros::Time last_time_; + std::vector<pr2_mechanism_model::JointState*> joints_; + std::vector<control_toolbox::Pid> pids_; + + std::vector<boost::shared_ptr<filters::FilterChain<double> > > output_filters_; + + ros::NodeHandle node_; + + void commandCB(const std_msgs::Float64MultiArray::ConstPtr &msg); + ros::Subscriber sub_command_; + + boost::scoped_ptr< + realtime_tools::RealtimePublisher< + pr2_controllers_msgs::JointTrajectoryControllerState> > controller_state_publisher_; + + // ------ Mechanisms for passing the commands into realtime + // (double buffering for commanded velocities, and guarded copy operation) + + boost::mutex command_mutex_; + std::vector<double> command_; +}; + +} + +#endif Property changes on: controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Modified: controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/rosie_odometry.h =================================================================== --- controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/rosie_odometry.h 2012-07-18 11:05:18 UTC (rev 1006) +++ controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/rosie_odometry.h 2012-08-07 14:36:57 UTC (rev 1007) @@ -36,7 +36,8 @@ This file is copied from pr2_mechanism_controllers. **/ -#include <Eigen/Array> +#define EIGEN2_SUPPORT + #include <Eigen/SVD> #include <nav_msgs/Odometry.h> #include <realtime_tools/realtime_publisher.h> Added: controllers/ias_mechanism_controllers/launch/pr2_loopback.launch =================================================================== --- controllers/ias_mechanism_controllers/launch/pr2_loopback.launch (rev 0) +++ controllers/ias_mechanism_controllers/launch/pr2_loopback.launch 2012-08-07 14:36:57 UTC (rev 1007) @@ -0,0 +1,29 @@ +<launch> + + <param name="/use_sim_time" value="false" /> + + <include file="$(find pr2_description)/robots/upload_pr2.launch" /> + + <!-- start loopback controller manager --> + <node name="loopback_controllers" type="loopback_controller_manager" pkg="loopback_controller_manager" output="screen"> + <param name="dt" value="0.002" /> + <param name="damping" value="1.0" /> + </node> + + <!-- start joint_states to tf transformer --> + <node pkg="robot_state_chain_publisher" type="state_chain_publisher" name="robot_state_publisher" output="log" /> + + +<!-- <node name="unspawn_arms" + pkg="pr2_controller_manager" type="unspawner" + args="l_arm_controller r_arm_controller" /> +--> + + <rosparam command="load" file="$(find ias_mechanism_controllers)/launch/pr2_multi_vel.yaml"/> + + + <node name="spawn_vel" + pkg="pr2_controller_manager" type="spawner" + args="l_arm_vel r_arm_vel" /> + +</launch> Added: controllers/ias_mechanism_controllers/launch/pr2_multi_vel.yaml =================================================================== --- controllers/ias_mechanism_controllers/launch/pr2_multi_vel.yaml (rev 0) +++ controllers/ias_mechanism_controllers/launch/pr2_multi_vel.yaml 2012-08-07 14:36:57 UTC (rev 1007) @@ -0,0 +1,39 @@ +r_arm_vel: + type: "ias_mechanism_controllers/MultiJointVelocityController" + joints: + - r_shoulder_pan_joint + - r_shoulder_lift_joint + - r_upper_arm_roll_joint + - r_elbow_flex_joint + - r_forearm_roll_joint + - r_wrist_flex_joint + - r_wrist_roll_joint + gains: + r_shoulder_pan_joint: {p: 4, d: 0.0} + r_shoulder_lift_joint: {p: 4, d: 0.0} + r_upper_arm_roll_joint: {p: 4, d: 0.0} + r_elbow_flex_joint: {p: 4, d: 0.0} + r_forearm_roll_joint: {p: 4, d: 0.0} + r_wrist_flex_joint: {p: 4, d: 0.0} + r_wrist_roll_joint: {p: 4, d: 0.0} + + +l_arm_vel: + type: "ias_mechanism_controllers/MultiJointVelocityController" + joints: + - l_shoulder_pan_joint + - l_shoulder_lift_joint + - l_upper_arm_roll_joint + - l_elbow_flex_joint + - l_forearm_roll_joint + - l_wrist_flex_joint + - l_wrist_roll_joint + gains: + l_shoulder_pan_joint: {p: 4, d: 0.0} + l_shoulder_lift_joint: {p: 4, d: 0.0} + l_upper_arm_roll_joint: {p: 4, d: 0.0} + l_elbow_flex_joint: {p: 4, d: 0.0} + l_forearm_roll_joint: {p: 4, d: 0.0} + l_wrist_flex_joint: {p: 4, d: 0.0} + l_wrist_roll_joint: {p: 4, d: 0.0} + Added: controllers/ias_mechanism_controllers/launch/rosie_loopback.launch =================================================================== --- controllers/ias_mechanism_controllers/launch/rosie_loopback.launch (rev 0) +++ controllers/ias_mechanism_controllers/launch/rosie_loopback.launch 2012-08-07 14:36:57 UTC (rev 1007) @@ -0,0 +1,29 @@ +<launch> + + <param name="/use_sim_time" value="false" /> + + <include file="$(find rosie_description)/launch/upload_rosie.launch" /> + + <!-- start loopback controller manager --> + <node name="loopback_controllers" type="loopback_controller_manager" pkg="loopback_controller_manager" output="screen"> + <param name="dt" value="0.002" /> + <param name="damping" value="1.0" /> + </node> + + <!-- start joint_states to tf transformer --> + <node pkg="robot_state_chain_publisher" type="state_chain_publisher" name="robot_state_publisher" output="log" /> + + +<!-- <node name="unspawn_arms" + pkg="pr2_controller_manager" type="unspawner" + args="l_arm_controller r_arm_controller" /> +--> + + <rosparam command="load" file="$(find ias_mechanism_controllers)/launch/rosie_multi_vel.yaml"/> + + + <node name="spawn_vel" + pkg="pr2_controller_manager" type="spawner" + args="l_arm_vel r_arm_vel" /> + +</launch> Added: controllers/ias_mechanism_controllers/launch/rosie_multi_vel.yaml =================================================================== --- controllers/ias_mechanism_controllers/launch/rosie_multi_vel.yaml (rev 0) +++ controllers/ias_mechanism_controllers/launch/rosie_multi_vel.yaml 2012-08-07 14:36:57 UTC (rev 1007) @@ -0,0 +1,39 @@ +r_arm_vel: + type: "ias_mechanism_controllers/MultiJointVelocityController" + joints: + - right_arm_0_joint + - right_arm_1_joint + - right_arm_2_joint + - right_arm_3_joint + - right_arm_4_joint + - right_arm_5_joint + - right_arm_6_joint + gains: + right_arm_0_joint: {p: 4, d: 0.0} + right_arm_1_joint: {p: 4, d: 0.0} + right_arm_2_joint: {p: 4, d: 0.0} + right_arm_3_joint: {p: 4, d: 0.0} + right_arm_4_joint: {p: 4, d: 0.0} + right_arm_5_joint: {p: 4, d: 0.0} + right_arm_6_joint: {p: 4, d: 0.0} + + +l_arm_vel: + type: "ias_mechanism_controllers/MultiJointVelocityController" + joints: + - left_arm_0_joint + - left_arm_1_joint + - left_arm_2_joint + - left_arm_3_joint + - left_arm_4_joint + - left_arm_5_joint + - left_arm_6_joint + gains: + left_arm_0_joint: {p: 4.0} + left_arm_1_joint: {p: 4.0} + left_arm_2_joint: {p: 4.0} + left_arm_3_joint: {p: 4.0} + left_arm_4_joint: {p: 4.0} + left_arm_5_joint: {p: 4.0} + left_arm_6_joint: {p: 4.0} + Modified: controllers/ias_mechanism_controllers/manifest.xml =================================================================== --- controllers/ias_mechanism_controllers/manifest.xml 2012-07-18 11:05:18 UTC (rev 1006) +++ controllers/ias_mechanism_controllers/manifest.xml 2012-08-07 14:36:57 UTC (rev 1007) @@ -1,11 +1,15 @@ <package> <description brief="ias_mechanism_controllers"> Mechanism controllers for ias related stuff + (Odometry publisher for simulated ROSIE and a + multi-joint velocity controller.) </description> - <author>Lorenz Moesenlechner</author> + <author>Lorenz Moesenlechner, Ingo Kresse</author> <license>BSD</license> <review status="unreviewed" notes=""/> + <depend package="pluginlib" /> + <depend package="roscpp" /> <depend package="std_msgs" /> <depend package="pr2_controller_interface" /> @@ -13,7 +17,9 @@ <depend package="pr2_mechanism_controllers" /> <depend package="pr2_mechanism_msgs" /> <depend package="nav_msgs" /> + <depend package="filters" /> <depend package="control_toolbox" /> + <depend package="realtime_tools" /> <depend package="pr2_controller_manager" /> <export> Added: controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp =================================================================== --- controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp (rev 0) +++ controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp 2012-08-07 14:36:57 UTC (rev 1007) @@ -0,0 +1,210 @@ +/* + * 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 "ias_mechanism_controllers/multi_joint_velocity_controller.h" +#include <sstream> +#include "angles/angles.h" +#include "pluginlib/class_list_macros.h" + +PLUGINLIB_DECLARE_CLASS(ias_mechanism_controllers, MultiJointVelocityController, controller::MultiJointVelocityController, pr2_controller_interface::Controller) + +namespace controller { + + +MultiJointVelocityController::MultiJointVelocityController() + : loop_count_(0), robot_(NULL) +{ +} + +MultiJointVelocityController::~MultiJointVelocityController() +{ + sub_command_.shutdown(); +} + +bool MultiJointVelocityController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) +{ + using namespace XmlRpc; + node_ = n; + robot_ = robot; + + // Gets all of the joints + XmlRpc::XmlRpcValue joint_names; + if (!node_.getParam("joints", joint_names)) + { + ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str()); + return false; + } + if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) + { + ROS_ERROR("Malformed joint specification. (namespace: %s)", node_.getNamespace().c_str()); + return false; + } + for (int i = 0; i < joint_names.size(); ++i) + { + XmlRpcValue &name_value = joint_names[i]; + if (name_value.getType() != XmlRpcValue::TypeString) + { + ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)", + node_.getNamespace().c_str()); + return false; + } + + pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value); + if (!j) { + ROS_ERROR("Joint not found: %s. (namespace: %s)", + ((std::string)name_value).c_str(), node_.getNamespace().c_str()); + return false; + } + joints_.push_back(j); + } + + // Ensures that all the joints are calibrated. + for (size_t i = 0; i < joints_.size(); ++i) + { + if (!joints_[i]->calibrated_) + { + ROS_ERROR("Joint %s was not calibrated (namespace: %s)", + joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str()); + return false; + } + } + + // Sets up pid controllers for all of the joints + std::string gains_ns; + if (!node_.getParam("gains", gains_ns)) + gains_ns = node_.getNamespace() + "/gains"; + pids_.resize(joints_.size()); + for (size_t i = 0; i < joints_.size(); ++i) + if (!pids_[i].init(ros::NodeHandle(gains_ns + "/" + joints_[i]->joint_->name))) + return false; + + + output_filters_.resize(joints_.size()); + for (size_t i = 0; i < joints_.size(); ++i) + { + std::string p = "output_filters/" + joints_[i]->joint_->name; + if (node_.hasParam(p)) + { + output_filters_[i].reset(new filters::FilterChain<double>("double")); + if (!output_filters_[i]->configure(node_.resolveName(p))) + output_filters_[i].reset(); + } + } + + sub_command_ = node_.subscribe("command", 1, &MultiJointVelocityController::commandCB, this); + + command_.resize(joints_.size()); + + controller_state_publisher_.reset( + new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointTrajectoryControllerState> + (node_, "state", 1)); + controller_state_publisher_->lock(); + for (size_t j = 0; j < joints_.size(); ++j) + controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name); + controller_state_publisher_->msg_.desired.velocities.resize(joints_.size()); + controller_state_publisher_->msg_.actual.positions.resize(joints_.size()); + controller_state_publisher_->msg_.actual.velocities.resize(joints_.size()); + controller_state_publisher_->msg_.error.velocities.resize(joints_.size()); + controller_state_publisher_->unlock(); + + + return true; +} + +void MultiJointVelocityController::starting() +{ + last_time_ = robot_->getTime(); + + for (size_t i = 0; i < pids_.size(); ++i) + pids_[i].reset(); + + // Creates a "hold current position" trajectory. +} + +void MultiJointVelocityController::update() +{ + ros::Time time = robot_->getTime(); + ros::Duration dt = time - last_time_; + last_time_ = time; + + double command[joints_.size()]; + + boost::mutex::scoped_lock guard(command_mutex_); + for(unsigned int i=0; i < command_.size(); ++i) + command[i] = command_[i]; + guard.unlock(); + + double error[joints_.size()]; + for (size_t i = 0; i < joints_.size(); ++i) + { + error[i] = joints_[i]->velocity_ - command[i]; + double effort = pids_[i].updatePid(error[i], dt); + double effort_filtered = effort; + if (output_filters_[i]) + output_filters_[i]->update(effort, effort_filtered); + joints_[i]->commanded_effort_ += effort_filtered; + } + + + // ------ State publishing + + if (loop_count_ % 10 == 0) + { + if (controller_state_publisher_ && controller_state_publisher_->trylock()) + { + controller_state_publisher_->msg_.header.stamp = time; + for (size_t j = 0; j < joints_.size(); ++j) + { + controller_state_publisher_->msg_.desired.velocities[j] = command[j]; + controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_; + controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_; + controller_state_publisher_->msg_.error.velocities[j] = error[j]; + } + controller_state_publisher_->unlockAndPublish(); + } + } + + ++loop_count_; +} + +void MultiJointVelocityController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &msg) +{ + boost::mutex::scoped_lock guard(command_mutex_); + for(unsigned int i=0; i < command_.size(); ++i) + command_[i] = msg->data[i]; +} + + + + +} Property changes on: controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Modified: controllers/ias_mechanism_controllers/src/rosie_odometry.cpp =================================================================== --- controllers/ias_mechanism_controllers/src/rosie_odometry.cpp 2012-07-18 11:05:18 UTC (rev 1006) +++ controllers/ias_mechanism_controllers/src/rosie_odometry.cpp 2012-08-07 14:36:57 UTC (rev 1007) @@ -38,7 +38,7 @@ #include "ias_mechanism_controllers/rosie_odometry.h" #include "pluginlib/class_list_macros.h" -PLUGINLIB_REGISTER_CLASS(RosieOdometry, controller::RosieOdometry, pr2_controller_interface::Controller) +PLUGINLIB_DECLARE_CLASS(ias_mechanism_controllers, RosieOdometry, controller::RosieOdometry, pr2_controller_interface::Controller) namespace controller { @@ -93,7 +93,7 @@ odometry_publisher_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(node_,odom_frame_, 1)); transform_publisher_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(node_,"/tf", 1)); - transform_publisher_->msg_.set_transforms_size(1); + transform_publisher_->msg_.transforms.resize(1); return true; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-07-18 11:05:29
|
Revision: 1006 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1006&view=rev Author: moesenle Date: 2012-07-18 11:05:18 +0000 (Wed, 18 Jul 2012) Log Message: ----------- Renamed cram_pl to cram_core. Modified Paths: -------------- rosinstall/tum-ros-pkg.rosinstall Modified: rosinstall/tum-ros-pkg.rosinstall =================================================================== --- rosinstall/tum-ros-pkg.rosinstall 2012-07-16 09:21:31 UTC (rev 1005) +++ rosinstall/tum-ros-pkg.rosinstall 2012-07-18 11:05:18 UTC (rev 1006) @@ -17,8 +17,8 @@ local-name: cram_physics uri: https://github.com/moesenle/cram_physics.git - git: - local-name: cram_pl - uri: http://code.in.tum.de/git/cram-pl.git + local-name: cram_core + uri: https://github.com/moesenle/cram_core.git - git: local-name: cram_highlevel uri: https://github.com/moesenle/cram_highlevel.git This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-07-16 09:21:40
|
Revision: 1005 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1005&view=rev Author: moesenle Date: 2012-07-16 09:21:31 +0000 (Mon, 16 Jul 2012) Log Message: ----------- Added repo pr2_led_kinect_calib Modified Paths: -------------- rosinstall/tum-ros-pkg.rosinstall Modified: rosinstall/tum-ros-pkg.rosinstall =================================================================== --- rosinstall/tum-ros-pkg.rosinstall 2012-07-13 11:23:09 UTC (rev 1004) +++ rosinstall/tum-ros-pkg.rosinstall 2012-07-16 09:21:31 UTC (rev 1005) @@ -34,6 +34,9 @@ - git: local-name: roll uri: http://code.in.tum.de/git/roll.git +- git: + local-name: pr2_led_kinect_calib + uri: https://github.com/ruehr/pr2_led_kinect_calib.git - git: local-name: uncertain_tf uri: https://github.com/ruehr/uncertain_tf.git This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-07-13 11:23:20
|
Revision: 1004 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1004&view=rev Author: moesenle Date: 2012-07-13 11:23:09 +0000 (Fri, 13 Jul 2012) Log Message: ----------- Author: Thomas Ruehr <ru...@in...> Date: Fri Jul 13 13:22:45 2012 +0200 scan_rate parameter. warnings. overhauled for roboearth workshop. commit 9d1ab6e850f0e8fb1e497e691c686817c3b5f2f6 Author: Thomas Ruehr <ru...@in...> Date: Tue Apr 26 19:16:32 2011 +0200 some demo scripting commit 1d198d168e12c3b68143cbafc09e611430e98504 Author: Thomas Ruehr <ru...@in...> Date: Wed Apr 20 11:16:35 2011 +0200 Republishes a PC2 on another topic, e.g. FreezeCloud2 /kinect/cloud_throttled /sample, this is useful for being able to deselect points in rviz (ctrl-klick/drag), since you cant deselect something that is not displayed anymore commit 6d63ae310c09135346b8b918477418f917e66d5d Author: Thomas Ruehr <ru...@in...> Date: Tue Apr 19 09:23:59 2011 +0200 all play and no work commit bba7b8cc4de9981b34436402c3132fb85e6f449f Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 31 10:38:18 2011 +0200 SnapMap->Diamondback(eigen namespace), OperatehandleController now extrapolates relative transform from start to current by 5cm to find next hand pose; added static tf publisher to provide fake odom for amcl when wheels are dead commit dc0014e9b51398cfd690f578b7e2d974c4b1b17e Author: Dejan Pangercic <dej...@gm...> Date: Wed Mar 23 15:23:51 2011 +0100 snap map compiles with eigen commit 7094fbd46755c501c9f62e7d16e6c794a43b807f Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 3 18:05:25 2011 +0100 moved 3d perception to new class commit 861fcc248bd1a1013d95289b6ddafe34b3da787e Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 3 15:22:05 2011 +0100 moved SnapMapICP out of ias_drawer_executive git-commit Modified Paths: -------------- highlevel/SnapMapICP/CMakeLists.txt highlevel/SnapMapICP/src/SnapMapICP.cpp Modified: highlevel/SnapMapICP/CMakeLists.txt =================================================================== --- highlevel/SnapMapICP/CMakeLists.txt 2012-06-21 12:21:05 UTC (rev 1003) +++ highlevel/SnapMapICP/CMakeLists.txt 2012-07-13 11:23:09 UTC (rev 1004) @@ -21,6 +21,10 @@ #uncomment if you have defined services #rosbuild_gensrv() +find_package(Eigen REQUIRED) +include_directories(${EIGEN_INCLUDE_DIRS}) +add_definitions(${EIGEN_DEFINITIONS}) + #common commands for building c++ executables and libraries #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) #target_link_libraries(${PROJECT_NAME} another_library) Modified: highlevel/SnapMapICP/src/SnapMapICP.cpp =================================================================== --- highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-06-21 12:21:05 UTC (rev 1003) +++ highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-07-13 11:23:09 UTC (rev 1004) @@ -71,6 +71,8 @@ double POSE_COVARIANCE_TRANS = 1.5; double ICP_NUM_ITER = 250; +double SCAN_RATE = 2; + std::string BASE_LASER_FRAME = "/base_laser_link"; std::string ODOM_FRAME = "/odom_combined"; @@ -137,7 +139,7 @@ btMatrix3x3 basis; basis.setFromOpenGLSubMatrix(mv); - btVector3 origin(out_mat (0, 3),out_mat (1, 3),out_mat (2, 3)); + tf::Vector3 origin(out_mat (0, 3),out_mat (1, 3),out_mat (2, 3)); ROS_DEBUG("origin %f %f %f", origin.x(), origin.y(), origin.z()); @@ -213,11 +215,13 @@ { bool gotTransform = false; - if (!listener_->waitForTransform(parent_frame, child_frame, stamp, ros::Duration(0.05))) + ros::Time before = ros::Time::now(); + if (!listener_->waitForTransform(parent_frame, child_frame, stamp, ros::Duration(0.5))) { ROS_WARN("DIDNT GET TRANSFORM %s %s IN c at %f", parent_frame.c_str(), child_frame.c_str(), stamp.toSec()); return false; } + //ROS_INFO("waited for transform %f", (ros::Time::now() - before).toSec()); try { @@ -234,19 +238,43 @@ return gotTransform; } +ros::Time last_processed_scan; void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in) { - if (!we_have_a_map) { ROS_INFO("SnapMapICP waiting for map to be published"); return; } + + ros::Time scan_in_time = scan_in->header.stamp; + ros::Time time_received = ros::Time::now(); + + if ( scan_in_time - last_processed_scan < ros::Duration(1.0f / SCAN_RATE) ) + { + ROS_DEBUG("rejected scan, last %f , this %f", last_processed_scan.toSec() ,scan_in_time.toSec()); + return; + } + + //projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_); if (!scan_callback_mutex.try_lock()) return; + ros::Duration scan_age = ros::Time::now() - scan_in_time; + + //check if we want to accept this scan, if its older than 1 sec, drop it + if (!use_sim_time) + if (scan_age.toSec() > AGE_THRESHOLD) + { + //ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD); + ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold) scan time: %f , now %f", scan_age.toSec(), AGE_THRESHOLD, scan_in_time.toSec(),ros::Time::now().toSec() ); + scan_callback_mutex.unlock(); + + return; + } + count_sc_++; //ROS_DEBUG("count_sc %i MUTEX LOCKED", count_sc_); @@ -255,8 +283,6 @@ { count_sc_ = 0; - ros::Time scan_in_time = scan_in->header.stamp; - tf::StampedTransform base_at_laser; if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time)) { @@ -266,18 +292,6 @@ return; } - // nh.param<std::string>("default_param", default_param, "default_value"); - nh->param<bool>("USE_SIM_TIME", use_sim_time, false); - nh->param<double>("SnapMapICP/icp_fitness_threshold", ICP_FITNESS_THRESHOLD, 100 ); - nh->param<double>("SnapMapICP/age_threshold", AGE_THRESHOLD, 1); - nh->param<double>("SnapMapICP/angle_upper_threshold", ANGLE_UPPER_THRESHOLD, 1); - nh->param<double>("SnapMapICP/angle_threshold", ANGLE_THRESHOLD, 0.01); - nh->param<double>("SnapMapICP/update_age_threshold", UPDATE_AGE_THRESHOLD, 1); - nh->param<double>("SnapMapICP/dist_threshold", DIST_THRESHOLD, 0.01); - nh->param<double>("SnapMapICP/icp_inlier_threshold", ICP_INLIER_THRESHOLD, 0.88); - nh->param<double>("SnapMapICP/icp_inlier_dist", ICP_INLIER_DIST, 0.1); - nh->param<double>("SnapMapICP/icp_num_iter", ICP_NUM_ITER, 250); - nh->param<double>("SnapMapICP/pose_covariance_trans", POSE_COVARIANCE_TRANS, 0.5); sensor_msgs::PointCloud cloud; sensor_msgs::PointCloud cloudInMap; @@ -354,6 +368,8 @@ reg.setMaximumIterations (ICP_NUM_ITER); // Set the point representation + //ros::Time bef = ros::Time::now(); + PointCloudT::Ptr myMapCloud (new PointCloudT()); PointCloudT::Ptr myScanCloud (new PointCloudT()); @@ -365,22 +381,22 @@ PointCloudT unused; int i = 0; - reg.align (unused); //-- running the registration in a loop to visualize intermediate results + reg.align (unused); + const Eigen::Matrix4f &transf = reg.getFinalTransformation(); tf::Transform t; matrixAsTransfrom(transf,t); + //ROS_ERROR("proc time %f", (ros::Time::now() - bef).toSec()); + we_have_a_scan_transformed = false; PointCloudT transformedCloud; pcl::transformPointCloud (*myScanCloud, transformedCloud, reg.getFinalTransformation()); - double inlier_perc = 0; - { // count inliers - std::vector<int> nn_indices (1); std::vector<float> nn_sqr_dists (1); @@ -393,17 +409,19 @@ } if (transformedCloud.points.size() > 0) { - ROS_DEBUG("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD); + //ROS_INFO("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD); inlier_perc = (double) numinliers / (double) transformedCloud.points.size(); } } + last_processed_scan = scan_in_time; + pcl::toROSMsg (transformedCloud, cloud2transformed); we_have_a_scan_transformed = true; double dist = sqrt((t.getOrigin().x() * t.getOrigin().x()) + (t.getOrigin().y() * t.getOrigin().y())); double angleDist = t.getRotation().getAngle(); - btVector3 rotAxis = t.getRotation().getAxis(); + tf::Vector3 rotAxis = t.getRotation().getAxis(); t = t * oldPose; tf::StampedTransform base_after_icp; @@ -423,8 +441,6 @@ //ROS_DEBUG("dist %f angleDist %f",dist, angleDist); - ros::Time scan_time = scan_in->header.stamp; - ros::Duration scan_age = ros::Time::now() - scan_time; //ROS_DEBUG("SCAN_AGE seems to be %f", scan_age.toSec()); char msg_c_str[2048]; sprintf(msg_c_str,"INLIERS %f (%f) scan_age %f (%f age_threshold) dist %f angleDist %f axis(%f %f %f) fitting %f (icp_fitness_threshold %f)",inlier_perc, ICP_INLIER_THRESHOLD, scan_age.toSec(), AGE_THRESHOLD ,dist, angleDist, rotAxis.x(), rotAxis.y(), rotAxis.z(),reg.getFitnessScore(), ICP_FITNESS_THRESHOLD ); @@ -433,18 +449,6 @@ //ROS_DEBUG("%s", msg_c_str); - - //check if we want to accept this scan, if its older than 1 sec, drop it - if (!use_sim_time) - if (scan_age.toSec() > AGE_THRESHOLD) - { - ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD); - scan_callback_mutex.unlock(); - - return; - } - - double cov = POSE_COVARIANCE_TRANS; //if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (angleDist < ANGLE_UPPER_THRESHOLD)) @@ -457,7 +461,7 @@ pose.pose.pose.position.x = t.getOrigin().x(); pose.pose.pose.position.y = t.getOrigin().y(); - btQuaternion quat = t.getRotation(); + tf::Quaternion quat = t.getRotation(); //quat.setRPY(0.0, 0.0, theta); tf::quaternionTFToMsg(quat,pose.pose.pose.orientation); float factorPos = 0.03; @@ -471,6 +475,8 @@ strmsg.data += " << SENT"; } + //ROS_INFO("processing time : %f", (ros::Time::now() - time_received).toSec()); + pub_info_.publish(strmsg); //ROS_DEBUG("map width %i height %i size %i, %s", myMapCloud.width, myMapCloud.height, (int)myMapCloud.points.size(), myMapCloud.header.frame_id.c_str()); //ROS_DEBUG("scan width %i height %i size %i, %s", myScanCloud.width, myScanCloud.height, (int)myScanCloud.points.size(), myScanCloud.header.frame_id.c_str()); @@ -479,6 +485,32 @@ scan_callback_mutex.unlock(); } + +ros::Time paramsWereUpdated; + + +void updateParams() +{ + paramsWereUpdated = ros::Time::now(); + // nh.param<std::string>("default_param", default_param, "default_value"); + nh->param<bool>("USE_SIM_TIME", use_sim_time, false); + nh->param<double>("SnapMapICP/icp_fitness_threshold", ICP_FITNESS_THRESHOLD, 100 ); + nh->param<double>("SnapMapICP/age_threshold", AGE_THRESHOLD, 1); + nh->param<double>("SnapMapICP/angle_upper_threshold", ANGLE_UPPER_THRESHOLD, 1); + nh->param<double>("SnapMapICP/angle_threshold", ANGLE_THRESHOLD, 0.01); + nh->param<double>("SnapMapICP/update_age_threshold", UPDATE_AGE_THRESHOLD, 1); + nh->param<double>("SnapMapICP/dist_threshold", DIST_THRESHOLD, 0.01); + nh->param<double>("SnapMapICP/icp_inlier_threshold", ICP_INLIER_THRESHOLD, 0.88); + nh->param<double>("SnapMapICP/icp_inlier_dist", ICP_INLIER_DIST, 0.1); + nh->param<double>("SnapMapICP/icp_num_iter", ICP_NUM_ITER, 250); + nh->param<double>("SnapMapICP/pose_covariance_trans", POSE_COVARIANCE_TRANS, 0.5); + nh->param<double>("SnapMapICP/scan_rate", SCAN_RATE, 2); + if (SCAN_RATE < .001) + SCAN_RATE = .001; + //ROS_DEBUG("PARAM UPDATE"); + +} + int main(int argc, char** argv) { @@ -490,6 +522,7 @@ nh->param<std::string>("SnapMapICP/odom_frame", ODOM_FRAME, "/odom_combined"); nh->param<std::string>("SnapMapICP/base_laser_frame", BASE_LASER_FRAME, "/base_laser_link"); + last_processed_scan = ros::Time::now(); projector_ = new laser_geometry::LaserProjection(); tf::TransformListener listener; @@ -501,10 +534,10 @@ pub_output_scan_transformed = nh->advertise<sensor_msgs::PointCloud2> ("scan_points_transformed", 1); pub_pose = nh->advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1); - ros::Subscriber subMap = nh_.subscribe("map", 10, mapCallback); - ros::Subscriber subScan = nh_.subscribe("base_scan", 10, scanCallback); + ros::Subscriber subMap = nh_.subscribe("map", 1, mapCallback); + ros::Subscriber subScan = nh_.subscribe("base_scan", 1, scanCallback); - ros::Rate loop_rate(10); + ros::Rate loop_rate(5); listener_->waitForTransform("/base_link", "/map", ros::Time(0), ros::Duration(30.0)); @@ -512,6 +545,14 @@ listener_->waitForTransform(BASE_LASER_FRAME, "/map", ros::Time(0), ros::Duration(30.0)); + ros::AsyncSpinner spinner(1); + spinner.start(); + + updateParams(); + + ROS_INFO("SnapMapICP running."); + + while (ros::ok()) { if (actScan > lastScan) @@ -529,6 +570,9 @@ } loop_rate.sleep(); ros::spinOnce(); + + if (ros::Time::now() - paramsWereUpdated > ros::Duration(1)) + updateParams(); } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-06-21 12:21:14
|
Revision: 1003 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1003&view=rev Author: blodow Date: 2012-06-21 12:21:05 +0000 (Thu, 21 Jun 2012) Log Message: ----------- added realtime_urdf_filter to tum-ros-pkg.rosinstall file Modified Paths: -------------- rosinstall/tum-ros-pkg.rosinstall Modified: rosinstall/tum-ros-pkg.rosinstall =================================================================== --- rosinstall/tum-ros-pkg.rosinstall 2012-06-12 15:53:08 UTC (rev 1002) +++ rosinstall/tum-ros-pkg.rosinstall 2012-06-21 12:21:05 UTC (rev 1003) @@ -37,3 +37,6 @@ - git: local-name: uncertain_tf uri: https://github.com/ruehr/uncertain_tf.git +- git: + local-name: realtime_urdf_filter + uri: https://github.com/blodow/realtime_urdf_filter.git This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-06-12 15:53:15
|
Revision: 1002 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1002&view=rev Author: moesenle Date: 2012-06-12 15:53:08 +0000 (Tue, 12 Jun 2012) Log Message: ----------- Author: Moritz Tenorth <te...@cs...> Date: Tue Jun 12 17:52:42 2012 +0200 commit 1272f340018a12a16e8706460faf8d5aa4c8e0d9 Author: Moritz Tenorth <te...@cs...> Date: Tue Jun 12 17:52:24 2012 +0200 added demo launch file commit 91784cef32b90c9bf91359f520171a9e4e5e8a9b Author: Moritz Tenorth <te...@cs...> Date: Tue Jun 12 14:03:11 2012 +0200 public export git-commit Added Paths: ----------- knowledge/knowrob_tutorial/launch/ knowledge/knowrob_tutorial/launch/tutorial.launch Added: knowledge/knowrob_tutorial/launch/tutorial.launch =================================================================== --- knowledge/knowrob_tutorial/launch/tutorial.launch (rev 0) +++ knowledge/knowrob_tutorial/launch/tutorial.launch 2012-06-12 15:53:08 UTC (rev 1002) @@ -0,0 +1,3 @@ +<launch> + <node name="json_prolog" pkg="rosprolog" type="run_with_prolog_env" args="ias_semantic_map $(find json_prolog)/bin/json_prolog" /> +</launch> \ 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: <tum...@li...> - 2012-04-23 09:30:57
|
Revision: 1000 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1000&view=rev Author: moesenle Date: 2012-04-23 09:30:47 +0000 (Mon, 23 Apr 2012) Log Message: ----------- added uncertain_tf stack to tum-ros-pkg Modified Paths: -------------- rosinstall/tum-ros-pkg.rosinstall Modified: rosinstall/tum-ros-pkg.rosinstall =================================================================== --- rosinstall/tum-ros-pkg.rosinstall 2012-04-18 15:57:41 UTC (rev 999) +++ rosinstall/tum-ros-pkg.rosinstall 2012-04-23 09:30:47 UTC (rev 1000) @@ -34,3 +34,6 @@ - git: local-name: roll uri: http://code.in.tum.de/git/roll.git +- git: + local-name: uncertain_tf + uri: https://github.com/ruehr/uncertain_tf.git This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-04-18 15:57:51
|
Revision: 999 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=999&view=rev Author: moesenle Date: 2012-04-18 15:57:41 +0000 (Wed, 18 Apr 2012) Log Message: ----------- Moved cram_highlevel to github. Modified Paths: -------------- rosinstall/tum-ros-pkg.rosinstall Modified: rosinstall/tum-ros-pkg.rosinstall =================================================================== --- rosinstall/tum-ros-pkg.rosinstall 2012-03-28 14:04:03 UTC (rev 998) +++ rosinstall/tum-ros-pkg.rosinstall 2012-04-18 15:57:41 UTC (rev 999) @@ -21,7 +21,7 @@ uri: http://code.in.tum.de/git/cram-pl.git - git: local-name: cram_highlevel - uri: http://code.in.tum.de/git/cram-highlevel.git + uri: https://github.com/moesenle/cram_highlevel.git - git: local-name: cram_rosie uri: http://code.in.tum.de/git/cram-rosie.git This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-03-28 14:04:13
|
Revision: 998 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=998&view=rev Author: moesenle Date: 2012-03-28 14:04:03 +0000 (Wed, 28 Mar 2012) Log Message: ----------- Removed perception/zbar_barcode_reader_node git-commit Removed Paths: ------------- perception/zbar_barcode_reader_node/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-03-28 14:04:00
|
Revision: 997 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=997&view=rev Author: moesenle Date: 2012-03-28 14:03:54 +0000 (Wed, 28 Mar 2012) Log Message: ----------- Removed 3rdparty/zbar git-commit Removed Paths: ------------- 3rdparty/zbar/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-02-24 00:37:52
|
Revision: 996 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=996&view=rev Author: moesenle Date: 2012-02-24 00:37:45 +0000 (Fri, 24 Feb 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@cs...> Date: Fri Feb 24 01:37:22 2012 +0100 commit cbb807243e3bbe09543973f9d8976c70f7904eab Author: Dejan Pangercic <dej...@cs...> Date: Fri Feb 24 01:36:20 2012 +0100 got rid of IplImage, needed to be tested commit 9d9f1f2127342dfece2d09aa07b355aa0cf4feaf Author: Dejan Pangercic <dej...@cs...> Date: Fri Feb 24 00:40:34 2012 +0100 ROS_INFO_STREAM, cv_bridge_ptr as class member commit 92834a4d47ff06e0809906e59bbddbfed2848ee9 Author: Dejan Pangercic <dej...@cs...> Date: Sat Feb 18 22:15:45 2012 +0100 public export git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp Modified: perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-02-18 21:16:22 UTC (rev 995) +++ perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-02-24 00:37:45 UTC (rev 996) @@ -49,12 +49,6 @@ BarcodeReaderNode(ros::NodeHandle &n) : n_(n), it_(n_) { - //link1 = "http://www.barcoo.com/api/get_product_complete?pi="; - //link2 = "&pins=ean&format=xml&source=ias-tum"; - //tag1 = "answer"; - //tag2 = "picture_high"; - //tag3 = "picture_low"; - n_.param ("input_image_topic", input_image_topic_, std::string("/image_raw")); n_.param ("outout_barcode_topic", output_barcode_topic_, std::string("barcode")); n_.param ("link1", link1_, std::string("http://www.barcoo.com/api/get_product_complete?pi=")); @@ -66,7 +60,6 @@ input_image_topic_, 1, &BarcodeReaderNode::imageCallback, this); barcode_pub_ = n_.advertise<std_msgs::String>(output_barcode_topic_, 1); - //cv::namedWindow ("Barcoo img"); enable_barcode_reader_ = 0; service_ = n.advertiseService("enable_barcode_reader_service", &BarcodeReaderNode::enable_barcode_reader,this); image_received_ = 0; @@ -221,15 +214,12 @@ //Check if there is a result std:: string result; - cout << "ohhh my goooooood\n\n"; findElement(&doc,result,tag1_); if(result.compare("0") == 0) //This condition checks if there is a result in the XML file if not returns 0 { - cout << "heloooooo barcode does not correspond to any object in the barcoo database"; + ROS_INFO_STREAM("Barcode does not correspond to any object in the barcoo database"); return 0; } - else - cout << "ELSE ELSE ELSE\n"; //Search for first tag findElement (&doc, picture,tag2_); @@ -333,15 +323,14 @@ if(!enable_barcode_reader_) return; - IplImage *cv_image = NULL; ROS_INFO("[BarcodeReaderNode: ] Image received"); try { - cv_image = bridge_.imgMsgToCv(msg_ptr, "mono8"); + cv_bridge_ptr_ = cv_bridge::toCvCopy(msg_ptr, "mono8"); } - catch (sensor_msgs::CvBridgeException error) + catch (cv_bridge::Exception& e) { - ROS_ERROR("error"); + ROS_ERROR("Error converting ROS Image"); } // create a reader @@ -350,12 +339,12 @@ // configure the reader scanner.set_config(ZBAR_NONE, ZBAR_CFG_ENABLE, 1); + int width = cv_bridge_ptr_->image.cols; // extract dimensions + int height = cv_bridge_ptr_->image.rows; + // obtain image data - // IplImage * cv_image = cvLoadImage(argv[1], CV_LOAD_IMAGE_GRAYSCALE); - Magick::Blob blob(cv_image->imageData, cv_image->imageSize); + Magick::Blob blob(cv_bridge_ptr_->image.ptr(0), width * height); - int width = cv_image->width; // extract dimensions - int height = cv_image->height; const void *raw = blob.data(); // wrap image data @@ -403,7 +392,7 @@ image_received_ = 1; return; } - // std::cerr << "buffer after callBarcoo " << buffer << std::endl; + //Search for info in the xml file std::string pictureLink; res = getPictureLink (buffer,pictureLink); @@ -411,7 +400,7 @@ if(res == 0) //If barcode does not exist in the database return { - cout << "Barcode does not exist in the barcoo datbase\n"; + ROS_INFO_STREAM("Barcode does not exist in the barcoo datbase"); image_received_ = 1; return; } @@ -423,7 +412,7 @@ std::string htmlPage; std::string htmlLink; findElement(&doc,htmlLink,"back_link"); - cout << "html link: "+htmlLink+"\n"; + ROS_INFO_STREAM ("html link: " << htmlLink); //Download HTML page getHTMLpage(htmlLink,htmlPage); std::string ss = pattern_; @@ -447,32 +436,19 @@ //publish image if (!(imgTmp.empty())) { - //imshow("Barcoo img", imgTmp); - cv_bridge::CvImagePtr cv_ptr (new cv_bridge::CvImage); - - std::cerr<< "\n\n\n\n\nTESTESTESTESTESTn\n\n\n\n"; - - cv_ptr->header.stamp = ros::Time::now(); - cv_ptr->header.frame_id = "frame"; - cv_ptr->encoding = "bgr8"; - std::cerr<< "\n\n\n\n\nHELO00000n\n\n\n\n"; - cv_ptr->image = imgTmp; - std::cerr<< "\n\n\n\n\nHELOn\n\n\n\n"; - - ros_image = cv_ptr->toImageMsg(); + cv_bridge_ptr_->header.stamp = ros::Time::now(); + cv_bridge_ptr_->header.frame_id = "frame"; + cv_bridge_ptr_->encoding = "bgr8"; + cv_bridge_ptr_->image = imgTmp; + ros_image = cv_bridge_ptr_->toImageMsg(); findElement(&doc,product_title,"title"); findElement(&doc,product_producer,"producer"); findElement(&doc,product_category,"category_key"); - - - image_found_ = 1; } image_received_ = 1; } cv::waitKey(3); - // clean up - //image.set_data(NULL, 0); } protected: @@ -488,6 +464,7 @@ std::string product_producer; std::string product_category; ros::ServiceServer service_; + cv_bridge::CvImagePtr cv_bridge_ptr_; }; int main(int argc, char** argv) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-02-18 21:16:29
|
Revision: 995 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=995&view=rev Author: moesenle Date: 2012-02-18 21:16:22 +0000 (Sat, 18 Feb 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@cs...> Date: Sat Feb 18 22:15:45 2012 +0100 commit 5cd7d5d41dca1d643ab5a61c680a35629b2f7573 Author: Dejan Pangercic <dej...@cs...> Date: Sat Feb 18 22:15:23 2012 +0100 forgotten include commit bc1d4b8e52767b7ad77db2b4e142f9f8ac5b4ec8 Author: Dejan Pangercic <dej...@cs...> Date: Sat Feb 18 22:09:16 2012 +0100 public export git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp Modified: perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-02-18 21:16:15 UTC (rev 994) +++ perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-02-18 21:16:22 UTC (rev 995) @@ -13,7 +13,7 @@ #include "tinyxml.h" #include <highgui.h> #include <zbar_barcode_reader_node/enable_barcode_reader.h> - +#include <cv_bridge/cv_bridge.h> //Magick++ lib #include <Magick++.h> //zbar This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-02-18 21:16:21
|
Revision: 994 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=994&view=rev Author: moesenle Date: 2012-02-18 21:16:15 +0000 (Sat, 18 Feb 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@cs...> Date: Sat Feb 18 22:09:16 2012 +0100 commit 15f57613e25e8ac1395a38f99f8bad776e07e8a4 Author: Dejan Pangercic <dej...@cs...> Date: Sat Feb 18 22:08:43 2012 +0100 new service definition and integration into the server commit d9f59658e46f74926677d131c1941e1fed7e9fdb Author: Dejan Pangercic <dej...@cs...> Date: Sun Feb 5 19:32:04 2012 +0100 public export git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/CMakeLists.txt perception/zbar_barcode_reader_node/manifest.xml perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp Added Paths: ----------- perception/zbar_barcode_reader_node/srv/ perception/zbar_barcode_reader_node/srv/enable_barcode_reader.srv Modified: perception/zbar_barcode_reader_node/CMakeLists.txt =================================================================== --- perception/zbar_barcode_reader_node/CMakeLists.txt 2012-02-14 13:27:57 UTC (rev 993) +++ perception/zbar_barcode_reader_node/CMakeLists.txt 2012-02-18 21:16:15 UTC (rev 994) @@ -17,7 +17,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) #uncomment if you have defined messages -#rosbuild_genmsg() +rosbuild_genmsg() #uncomment if you have defined services rosbuild_gensrv() Modified: perception/zbar_barcode_reader_node/manifest.xml =================================================================== --- perception/zbar_barcode_reader_node/manifest.xml 2012-02-14 13:27:57 UTC (rev 993) +++ perception/zbar_barcode_reader_node/manifest.xml 2012-02-18 21:16:15 UTC (rev 994) @@ -12,9 +12,10 @@ <depend package="opencv2"/> <depend package="cv_bridge"/> <depend package="image_transport"/> - <depend package="sensor_msgs"/> + <depend package="sensor_msgs"/> <rosdep name="libcurl"/> - <rosdep name="libtinyxml"/> + <rosdep name="libtinyxml"/> + </package> Modified: perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-02-14 13:27:57 UTC (rev 993) +++ perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-02-18 21:16:15 UTC (rev 994) @@ -31,7 +31,8 @@ std:: string pattern_;// = "<meta property=\"og:image\" content=\""; TiXmlDocument doc; int enable_barcode_reader_; - + int image_received_; + int image_found_; public: struct memoryStruct { char *memory; @@ -65,9 +66,10 @@ input_image_topic_, 1, &BarcodeReaderNode::imageCallback, this); barcode_pub_ = n_.advertise<std_msgs::String>(output_barcode_topic_, 1); - cv::namedWindow ("Barcoo img"); + //cv::namedWindow ("Barcoo img"); enable_barcode_reader_ = 0; service_ = n.advertiseService("enable_barcode_reader_service", &BarcodeReaderNode::enable_barcode_reader,this); + image_received_ = 0; } ~BarcodeReaderNode() @@ -78,8 +80,31 @@ bool enable_barcode_reader(zbar_barcode_reader_node::enable_barcode_reader::Request &req, zbar_barcode_reader_node::enable_barcode_reader::Response &res ) { - enable_barcode_reader_ = req.enable; - return true; + enable_barcode_reader_ = req.enable; + while(!image_received_) + { + sleep(1); + } + + if(image_found_) + { + res.title.data = product_title; + res.subtitle.data = product_producer; + res.category_key.data = product_category; + res.image_msg = *ros_image; + enable_barcode_reader_ = 0; + image_received_ = 0; + image_found_ = 0; + enable_barcode_reader_ = 0; + + return true; + } + else + { + image_received_ = 0; + enable_barcode_reader_ = 0; + return false; + } } static void* CURL_realloc(void *ptr, size_t size) @@ -359,12 +384,14 @@ if (n == 0) { ROS_WARN("Barcode not found"); + image_received_ = 1; return; } if (n < 0) { ROS_ERROR("Error occured while finding barcode"); + image_received_ = 1; return; } @@ -372,7 +399,10 @@ //Get xml file from barcoo database int res = getBarcooXML(ss.str(), buffer); if(res != 1) + { + image_received_ = 1; return; + } // std::cerr << "buffer after callBarcoo " << buffer << std::endl; //Search for info in the xml file std::string pictureLink; @@ -382,6 +412,7 @@ if(res == 0) //If barcode does not exist in the database return { cout << "Barcode does not exist in the barcoo datbase\n"; + image_received_ = 1; return; } if(res == -1) //This condition is true when the image link is not given @@ -413,14 +444,35 @@ cv::Mat imgTmp; // image object getImage (pictureLink ,& imgTmp); // display image + //publish image if (!(imgTmp.empty())) { - imshow("Barcoo img", imgTmp); + //imshow("Barcoo img", imgTmp); + cv_bridge::CvImagePtr cv_ptr (new cv_bridge::CvImage); + + std::cerr<< "\n\n\n\n\nTESTESTESTESTESTn\n\n\n\n"; + + cv_ptr->header.stamp = ros::Time::now(); + cv_ptr->header.frame_id = "frame"; + cv_ptr->encoding = "bgr8"; + std::cerr<< "\n\n\n\n\nHELO00000n\n\n\n\n"; + cv_ptr->image = imgTmp; + std::cerr<< "\n\n\n\n\nHELOn\n\n\n\n"; + + ros_image = cv_ptr->toImageMsg(); + findElement(&doc,product_title,"title"); + findElement(&doc,product_producer,"producer"); + findElement(&doc,product_category,"category_key"); + + + + image_found_ = 1; } + image_received_ = 1; } cv::waitKey(3); // clean up - image.set_data(NULL, 0); + //image.set_data(NULL, 0); } protected: @@ -431,14 +483,21 @@ ros::Publisher barcode_pub_; sensor_msgs::CvBridge bridge_; std::string input_image_topic_, output_barcode_topic_; + sensor_msgs::ImagePtr ros_image; + std::string product_title; + std::string product_producer; + std::string product_category; ros::ServiceServer service_; }; int main(int argc, char** argv) { ros::init(argc, argv, "barcode_reader_node"); + ros::AsyncSpinner spinner(1); // Use 4 threads + spinner.start(); ros::NodeHandle n("~"); BarcodeReaderNode br(n); ros::spin(); + return 0; } Added: perception/zbar_barcode_reader_node/srv/enable_barcode_reader.srv =================================================================== --- perception/zbar_barcode_reader_node/srv/enable_barcode_reader.srv (rev 0) +++ perception/zbar_barcode_reader_node/srv/enable_barcode_reader.srv 2012-02-18 21:16:15 UTC (rev 994) @@ -0,0 +1,6 @@ +int64 enable +--- +std_msgs/String title +std_msgs/String subtitle +std_msgs/String category_key +sensor_msgs/Image image_msg \ 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: <tum...@li...> - 2012-02-14 13:28:08
|
Revision: 993 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=993&view=rev Author: moesenle Date: 2012-02-14 13:27:57 +0000 (Tue, 14 Feb 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@gm...> Date: Tue Feb 14 14:27:37 2012 +0100 commit 87397cf1d1b5ccd96507252dcc26e0379750e2ba Author: Dejan Pangercic <dej...@gm...> Date: Tue Feb 14 14:27:08 2012 +0100 server side of service for trigering the barcode recognition from a QT client commit d9f59658e46f74926677d131c1941e1fed7e9fdb Author: Dejan Pangercic <dej...@cs...> Date: Sun Feb 5 19:32:04 2012 +0100 public export git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/CMakeLists.txt perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp Modified: perception/zbar_barcode_reader_node/CMakeLists.txt =================================================================== --- perception/zbar_barcode_reader_node/CMakeLists.txt 2012-02-13 09:08:17 UTC (rev 992) +++ perception/zbar_barcode_reader_node/CMakeLists.txt 2012-02-14 13:27:57 UTC (rev 993) @@ -19,7 +19,7 @@ #uncomment if you have defined messages #rosbuild_genmsg() #uncomment if you have defined services -#rosbuild_gensrv() +rosbuild_gensrv() #common commands for building c++ executables and libraries #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) Modified: perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-02-13 09:08:17 UTC (rev 992) +++ perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-02-14 13:27:57 UTC (rev 993) @@ -12,6 +12,7 @@ #include "curl/curl.h" #include "tinyxml.h" #include <highgui.h> +#include <zbar_barcode_reader_node/enable_barcode_reader.h> //Magick++ lib #include <Magick++.h> @@ -29,6 +30,7 @@ std:: string tag3_;// = "picture_low"; std:: string pattern_;// = "<meta property=\"og:image\" content=\""; TiXmlDocument doc; + int enable_barcode_reader_; public: struct memoryStruct { @@ -64,12 +66,22 @@ barcode_pub_ = n_.advertise<std_msgs::String>(output_barcode_topic_, 1); cv::namedWindow ("Barcoo img"); + enable_barcode_reader_ = 0; + service_ = n.advertiseService("enable_barcode_reader_service", &BarcodeReaderNode::enable_barcode_reader,this); } ~BarcodeReaderNode() { cv::destroyAllWindows(); } + + bool enable_barcode_reader(zbar_barcode_reader_node::enable_barcode_reader::Request &req, + zbar_barcode_reader_node::enable_barcode_reader::Response &res ) + { + enable_barcode_reader_ = req.enable; + return true; + } + static void* CURL_realloc(void *ptr, size_t size) { /* There might be a realloc() out there that doesn't like reallocing @@ -79,7 +91,7 @@ else return malloc(size); } - + static size_t WriteMemoryCallback (void *ptr, size_t size, size_t nmemb, void *data) { @@ -290,10 +302,12 @@ } return -1; } - + void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr) { - + + if(!enable_barcode_reader_) + return; IplImage *cv_image = NULL; ROS_INFO("[BarcodeReaderNode: ] Image received"); try @@ -417,6 +431,7 @@ ros::Publisher barcode_pub_; sensor_msgs::CvBridge bridge_; std::string input_image_topic_, output_barcode_topic_; + ros::ServiceServer service_; }; int main(int argc, char** argv) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-02-13 09:08:26
|
Revision: 992 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=992&view=rev Author: moesenle Date: 2012-02-13 09:08:17 +0000 (Mon, 13 Feb 2012) Log Message: ----------- Author: Thomas Ruehr <ru...@in...> Date: Mon Feb 13 10:08:04 2012 +0100 warnings. overhauled for roboearth workshop. commit 9d1ab6e850f0e8fb1e497e691c686817c3b5f2f6 Author: Thomas Ruehr <ru...@in...> Date: Tue Apr 26 19:16:32 2011 +0200 some demo scripting commit 1d198d168e12c3b68143cbafc09e611430e98504 Author: Thomas Ruehr <ru...@in...> Date: Wed Apr 20 11:16:35 2011 +0200 Republishes a PC2 on another topic, e.g. FreezeCloud2 /kinect/cloud_throttled /sample, this is useful for being able to deselect points in rviz (ctrl-klick/drag), since you cant deselect something that is not displayed anymore commit 6d63ae310c09135346b8b918477418f917e66d5d Author: Thomas Ruehr <ru...@in...> Date: Tue Apr 19 09:23:59 2011 +0200 all play and no work commit bba7b8cc4de9981b34436402c3132fb85e6f449f Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 31 10:38:18 2011 +0200 SnapMap->Diamondback(eigen namespace), OperatehandleController now extrapolates relative transform from start to current by 5cm to find next hand pose; added static tf publisher to provide fake odom for amcl when wheels are dead commit dc0014e9b51398cfd690f578b7e2d974c4b1b17e Author: Dejan Pangercic <dej...@gm...> Date: Wed Mar 23 15:23:51 2011 +0100 snap map compiles with eigen commit 7094fbd46755c501c9f62e7d16e6c794a43b807f Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 3 18:05:25 2011 +0100 moved 3d perception to new class commit 861fcc248bd1a1013d95289b6ddafe34b3da787e Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 3 15:22:05 2011 +0100 moved SnapMapICP out of ias_drawer_executive git-commit Modified Paths: -------------- highlevel/SnapMapICP/src/SnapMapICP.cpp Modified: highlevel/SnapMapICP/src/SnapMapICP.cpp =================================================================== --- highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-02-10 13:18:43 UTC (rev 991) +++ highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-02-13 09:08:17 UTC (rev 992) @@ -215,7 +215,7 @@ if (!listener_->waitForTransform(parent_frame, child_frame, stamp, ros::Duration(0.05))) { - ROS_ERROR("DIDNT GET TRANSFORM %s %s IN c at %f", parent_frame.c_str(), child_frame.c_str(), stamp.toSec()); + ROS_WARN("DIDNT GET TRANSFORM %s %s IN c at %f", parent_frame.c_str(), child_frame.c_str(), stamp.toSec()); return false; } @@ -227,7 +227,7 @@ catch (tf::TransformException ex) { gotTransform = false; - ROS_ERROR("DIDNT GET TRANSFORM %s %s IN B", parent_frame.c_str(), child_frame.c_str()); + ROS_WARN("DIDNT GET TRANSFORM %s %s IN B", parent_frame.c_str(), child_frame.c_str()); } @@ -240,7 +240,7 @@ if (!we_have_a_map) { - ROS_INFO("Waiting for map"); + ROS_INFO("SnapMapICP waiting for map to be published"); return; } //projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_); @@ -248,7 +248,7 @@ return; count_sc_++; - //ROS_INFO("count_sc %i MUTEX LOCKED", count_sc_); + //ROS_DEBUG("count_sc %i MUTEX LOCKED", count_sc_); //if (count_sc_ > 10) //if (count_sc_ > 10) @@ -260,7 +260,7 @@ tf::StampedTransform base_at_laser; if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time)) { - ROS_ERROR("Did not get base pose at laser scan time"); + ROS_WARN("Did not get base pose at laser scan time"); scan_callback_mutex.unlock(); return; @@ -312,7 +312,7 @@ catch (...) { gotTransform = false; - ROS_ERROR("DIDNT GET TRANSFORM IN A"); + ROS_WARN("DIDNT GET TRANSFORM IN A"); } } @@ -335,7 +335,7 @@ catch (tf::TransformException ex) { gotTransform = false; - ROS_ERROR("DIDNT GET TRANSFORM IN B"); + ROS_WARN("DIDNT GET TRANSFORM IN B"); } } if (we_have_a_map && gotTransform) @@ -393,7 +393,7 @@ } if (transformedCloud.points.size() > 0) { - ROS_WARN("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD); + ROS_DEBUG("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD); inlier_perc = (double) numinliers / (double) transformedCloud.points.size(); } } @@ -409,7 +409,7 @@ tf::StampedTransform base_after_icp; if (!getTransform(base_after_icp, ODOM_FRAME, "base_link", ros::Time(0))) { - ROS_ERROR("Did not get base pose at now"); + ROS_WARN("Did not get base pose at now"); scan_callback_mutex.unlock(); return; @@ -417,28 +417,28 @@ else { tf::Transform rel = base_at_laser.inverseTimes(base_after_icp); - ROS_INFO("relative motion of robot while doing icp: %fcm %fdeg", rel.getOrigin().length(), rel.getRotation().getAngle() * 180 / M_PI); + ROS_DEBUG("relative motion of robot while doing icp: %fcm %fdeg", rel.getOrigin().length(), rel.getRotation().getAngle() * 180 / M_PI); t= t * rel; } - //ROS_INFO("dist %f angleDist %f",dist, angleDist); + //ROS_DEBUG("dist %f angleDist %f",dist, angleDist); ros::Time scan_time = scan_in->header.stamp; ros::Duration scan_age = ros::Time::now() - scan_time; - //ROS_INFO("SCAN_AGE seems to be %f", scan_age.toSec()); + //ROS_DEBUG("SCAN_AGE seems to be %f", scan_age.toSec()); char msg_c_str[2048]; sprintf(msg_c_str,"INLIERS %f (%f) scan_age %f (%f age_threshold) dist %f angleDist %f axis(%f %f %f) fitting %f (icp_fitness_threshold %f)",inlier_perc, ICP_INLIER_THRESHOLD, scan_age.toSec(), AGE_THRESHOLD ,dist, angleDist, rotAxis.x(), rotAxis.y(), rotAxis.z(),reg.getFitnessScore(), ICP_FITNESS_THRESHOLD ); std_msgs::String strmsg; strmsg.data = msg_c_str; - //ROS_INFO("%s", msg_c_str); + //ROS_DEBUG("%s", msg_c_str); //check if we want to accept this scan, if its older than 1 sec, drop it if (!use_sim_time) if (scan_age.toSec() > AGE_THRESHOLD) { - ROS_ERROR("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD); + ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD); scan_callback_mutex.unlock(); return; @@ -465,8 +465,8 @@ pose.pose.covariance[6*0+0] = (cov * cov) * factorPos; pose.pose.covariance[6*1+1] = (cov * cov) * factorPos; pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot; - ROS_INFO("i %i converged %i SCORE: %f", i, reg.hasConverged (), reg.getFitnessScore() ); - ROS_INFO("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f [frame=%s]",dist, angleDist , pose.pose.pose.position.x , pose.pose.pose.position.y , pose.header.frame_id.c_str()); + ROS_DEBUG("i %i converged %i SCORE: %f", i, reg.hasConverged (), reg.getFitnessScore() ); + ROS_DEBUG("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f [frame=%s]",dist, angleDist , pose.pose.pose.position.x , pose.pose.pose.position.y , pose.header.frame_id.c_str()); pub_pose.publish(pose); strmsg.data += " << SENT"; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-02-10 13:18:50
|
Revision: 991 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=991&view=rev Author: moesenle Date: 2012-02-10 13:18:43 +0000 (Fri, 10 Feb 2012) Log Message: ----------- Author: Thomas Ruehr <ru...@in...> Date: Fri Feb 10 14:18:23 2012 +0100 overhauled for roboearth workshop. commit 9d1ab6e850f0e8fb1e497e691c686817c3b5f2f6 Author: Thomas Ruehr <ru...@in...> Date: Tue Apr 26 19:16:32 2011 +0200 some demo scripting commit 1d198d168e12c3b68143cbafc09e611430e98504 Author: Thomas Ruehr <ru...@in...> Date: Wed Apr 20 11:16:35 2011 +0200 Republishes a PC2 on another topic, e.g. FreezeCloud2 /kinect/cloud_throttled /sample, this is useful for being able to deselect points in rviz (ctrl-klick/drag), since you cant deselect something that is not displayed anymore commit 6d63ae310c09135346b8b918477418f917e66d5d Author: Thomas Ruehr <ru...@in...> Date: Tue Apr 19 09:23:59 2011 +0200 all play and no work commit bba7b8cc4de9981b34436402c3132fb85e6f449f Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 31 10:38:18 2011 +0200 SnapMap->Diamondback(eigen namespace), OperatehandleController now extrapolates relative transform from start to current by 5cm to find next hand pose; added static tf publisher to provide fake odom for amcl when wheels are dead commit dc0014e9b51398cfd690f578b7e2d974c4b1b17e Author: Dejan Pangercic <dej...@gm...> Date: Wed Mar 23 15:23:51 2011 +0100 snap map compiles with eigen commit 7094fbd46755c501c9f62e7d16e6c794a43b807f Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 3 18:05:25 2011 +0100 moved 3d perception to new class commit 861fcc248bd1a1013d95289b6ddafe34b3da787e Author: Thomas Ruehr <ru...@in...> Date: Thu Mar 3 15:22:05 2011 +0100 moved SnapMapICP out of ias_drawer_executive git-commit Modified Paths: -------------- highlevel/SnapMapICP/manifest.xml highlevel/SnapMapICP/src/SnapMapICP.cpp Modified: highlevel/SnapMapICP/manifest.xml =================================================================== --- highlevel/SnapMapICP/manifest.xml 2012-02-09 10:13:47 UTC (rev 990) +++ highlevel/SnapMapICP/manifest.xml 2012-02-10 13:18:43 UTC (rev 991) @@ -8,6 +8,7 @@ <license>BSD</license> <review status="unreviewed" notes=""/> <url>http://ros.org/wiki/SnapMapICP</url> + <depend package="std_msgs"/> <depend package="pcl"/> <depend package="nav_msgs"/> <depend package="laser_geometry"/> Modified: highlevel/SnapMapICP/src/SnapMapICP.cpp =================================================================== --- highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-02-09 10:13:47 UTC (rev 990) +++ highlevel/SnapMapICP/src/SnapMapICP.cpp 2012-02-10 13:18:43 UTC (rev 991) @@ -29,6 +29,7 @@ #include <ros/ros.h> +#include <std_msgs/String.h> #include <sensor_msgs/PointCloud.h> #include <sensor_msgs/PointCloud2.h> #include <nav_msgs/OccupancyGrid.h> @@ -44,22 +45,40 @@ #include <pcl/registration/icp.h> #include <pcl/registration/icp_nl.h> #include <pcl/registration/transforms.h> +#include <pcl/kdtree/kdtree_flann.h> #include <geometry_msgs/PoseWithCovarianceStamped.h> +#include <boost/thread/mutex.hpp> + +boost::mutex scan_callback_mutex; + //these should be parameters // defines how good the match has to be to create a candidate for publishing a pose -float ICP_FITNESS_THRESHOLD = 100.1;// = 0.025; +double ICP_FITNESS_THRESHOLD = 100.1;// = 0.025; //defines how much distance deviation from amcl to icp pose is needed to make us publish a pose -float DIST_THRESHOLD = 0.05; +double DIST_THRESHOLD = 0.05; // same for angle -float ANGLE_THRESHOLD = 0.01; +double ANGLE_THRESHOLD = 0.01; +double ANGLE_UPPER_THRESHOLD = M_PI / 6; +// accept only scans one second old or younger +double AGE_THRESHOLD = 1; +double UPDATE_AGE_THRESHOLD = 1; +double ICP_INLIER_THRESHOLD = 0.9; +double ICP_INLIER_DIST = 0.1; +double POSE_COVARIANCE_TRANS = 1.5; +double ICP_NUM_ITER = 250; + +std::string BASE_LASER_FRAME = "/base_laser_link"; +std::string ODOM_FRAME = "/odom_combined"; + ros::NodeHandle *nh = 0; ros::Publisher pub_output_; ros::Publisher pub_output_scan; ros::Publisher pub_output_scan_transformed; +ros::Publisher pub_info_; ros::Publisher pub_pose; @@ -78,6 +97,8 @@ bool we_have_a_scan = false; bool we_have_a_scan_transformed = false; +bool use_sim_time = false; + int lastScan = 0; int actScan = 0; @@ -124,6 +145,21 @@ } +boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_xyz; + +pcl::KdTree<pcl::PointXYZ>::Ptr mapTree; + + +pcl::KdTree<pcl::PointXYZ>::Ptr getTree(pcl::PointCloud<pcl::PointXYZ>::Ptr cloudb) +{ + pcl::KdTree<pcl::PointXYZ>::Ptr tree; + tree.reset (new pcl::KdTreeFLANN<pcl::PointXYZ>); + + tree->setInputCloud (cloudb); + return tree; +} + + void mapCallback(const nav_msgs::OccupancyGrid& msg) { ROS_DEBUG("I heard frame_id: [%s]", msg.header.frame_id.c_str()); @@ -135,7 +171,7 @@ float posx = msg.info.origin.position.x; float posy = msg.info.origin.position.y; - boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > cloud_xyz = boost::shared_ptr< pcl::PointCloud<pcl::PointXYZ> >(new pcl::PointCloud<pcl::PointXYZ>()); + cloud_xyz = boost::shared_ptr< pcl::PointCloud<pcl::PointXYZ> >(new pcl::PointCloud<pcl::PointXYZ>()); //cloud_xyz->width = 100; // 100 cloud_xyz->height = 1; @@ -160,6 +196,8 @@ } cloud_xyz->width = cloud_xyz->points.size(); + mapTree = getTree(cloud_xyz); + pcl::toROSMsg (*cloud_xyz, *output_cloud); ROS_DEBUG("Publishing PointXYZ cloud with %ld points in frame %s", cloud_xyz->points.size(),output_cloud->header.frame_id.c_str()); @@ -171,16 +209,76 @@ int count_sc_ = 0; +bool getTransform(tf::StampedTransform &trans , const std::string parent_frame, const std::string child_frame, const ros::Time stamp) +{ + bool gotTransform = false; + + if (!listener_->waitForTransform(parent_frame, child_frame, stamp, ros::Duration(0.05))) + { + ROS_ERROR("DIDNT GET TRANSFORM %s %s IN c at %f", parent_frame.c_str(), child_frame.c_str(), stamp.toSec()); + return false; + } + + try + { + gotTransform = true; + listener_->lookupTransform(parent_frame,child_frame,stamp , trans); + } + catch (tf::TransformException ex) + { + gotTransform = false; + ROS_ERROR("DIDNT GET TRANSFORM %s %s IN B", parent_frame.c_str(), child_frame.c_str()); + } + + + return gotTransform; +} + + void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in) { + if (!we_have_a_map) + { + ROS_INFO("Waiting for map"); + return; + } //projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_); + if (!scan_callback_mutex.try_lock()) + return; + count_sc_++; - if (count_sc_ > 10) + //ROS_INFO("count_sc %i MUTEX LOCKED", count_sc_); + + //if (count_sc_ > 10) + //if (count_sc_ > 10) { count_sc_ = 0; + ros::Time scan_in_time = scan_in->header.stamp; + tf::StampedTransform base_at_laser; + if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time)) + { + ROS_ERROR("Did not get base pose at laser scan time"); + scan_callback_mutex.unlock(); + + return; + } + + // nh.param<std::string>("default_param", default_param, "default_value"); + nh->param<bool>("USE_SIM_TIME", use_sim_time, false); + nh->param<double>("SnapMapICP/icp_fitness_threshold", ICP_FITNESS_THRESHOLD, 100 ); + nh->param<double>("SnapMapICP/age_threshold", AGE_THRESHOLD, 1); + nh->param<double>("SnapMapICP/angle_upper_threshold", ANGLE_UPPER_THRESHOLD, 1); + nh->param<double>("SnapMapICP/angle_threshold", ANGLE_THRESHOLD, 0.01); + nh->param<double>("SnapMapICP/update_age_threshold", UPDATE_AGE_THRESHOLD, 1); + nh->param<double>("SnapMapICP/dist_threshold", DIST_THRESHOLD, 0.01); + nh->param<double>("SnapMapICP/icp_inlier_threshold", ICP_INLIER_THRESHOLD, 0.88); + nh->param<double>("SnapMapICP/icp_inlier_dist", ICP_INLIER_DIST, 0.1); + nh->param<double>("SnapMapICP/icp_num_iter", ICP_NUM_ITER, 250); + nh->param<double>("SnapMapICP/pose_covariance_trans", POSE_COVARIANCE_TRANS, 0.5); + sensor_msgs::PointCloud cloud; sensor_msgs::PointCloud cloudInMap; @@ -188,6 +286,22 @@ we_have_a_scan = false; bool gotTransform = false; + + if (!listener_->waitForTransform("/map", cloud.header.frame_id, cloud.header.stamp, ros::Duration(0.05))) + { + scan_callback_mutex.unlock(); + ROS_WARN("SnapMapICP no map to cloud transform found MUTEX UNLOCKED"); + return; + } + + if (!listener_->waitForTransform("/map", "/base_link", cloud.header.stamp, ros::Duration(0.05))) + { + scan_callback_mutex.unlock(); + ROS_WARN("SnapMapICP no map to base transform found MUTEX UNLOCKED"); + return; + } + + while (!gotTransform && (ros::ok())) { try @@ -198,9 +312,16 @@ catch (...) { gotTransform = false; + ROS_ERROR("DIDNT GET TRANSFORM IN A"); } } + for (size_t k =0; k < cloudInMap.points.size(); k++) + { + cloudInMap.points[k].z = 0; + } + + gotTransform = false; tf::StampedTransform oldPose; while (!gotTransform && (ros::ok())) @@ -208,12 +329,13 @@ try { gotTransform = true; - listener_->lookupTransform("/map", "/base_footprint", - ros::Time(0), oldPose); + listener_->lookupTransform("/map", "/base_link", + cloud.header.stamp , oldPose); } catch (tf::TransformException ex) { gotTransform = false; + ROS_ERROR("DIDNT GET TRANSFORM IN B"); } } if (we_have_a_map && gotTransform) @@ -228,68 +350,133 @@ reg.setTransformationEpsilon (1e-6); // Set the maximum distance between two correspondences (src<->tgt) to 10cm // Note: adjust this based on the size of your datasets - reg.setMaxCorrespondenceDistance(0.5); - reg.setMaximumIterations (100); + reg.setMaxCorrespondenceDistance(1.5); + reg.setMaximumIterations (ICP_NUM_ITER); // Set the point representation - PointCloudT myMapCloud; - PointCloudT myScanCloud; + PointCloudT::Ptr myMapCloud (new PointCloudT()); + PointCloudT::Ptr myScanCloud (new PointCloudT()); - pcl::fromROSMsg(*output_cloud,myMapCloud); - pcl::fromROSMsg(cloud2,myScanCloud); + pcl::fromROSMsg(*output_cloud,*myMapCloud); + pcl::fromROSMsg(cloud2,*myScanCloud); - reg.setInputCloud(boost::make_shared< const PointCloudT > (myScanCloud)); - reg.setInputTarget(boost::make_shared< const PointCloudT > (myMapCloud)); + reg.setInputCloud(myScanCloud); + reg.setInputTarget(myMapCloud); PointCloudT unused; int i = 0; reg.align (unused); //-- running the registration in a loop to visualize intermediate results - Eigen::Matrix4f transf = reg.getFinalTransformation(); + const Eigen::Matrix4f &transf = reg.getFinalTransformation(); tf::Transform t; matrixAsTransfrom(transf,t); we_have_a_scan_transformed = false; PointCloudT transformedCloud; - pcl::transformPointCloud (myScanCloud, transformedCloud, reg.getFinalTransformation()); + pcl::transformPointCloud (*myScanCloud, transformedCloud, reg.getFinalTransformation()); + + double inlier_perc = 0; + + { + // count inliers + + std::vector<int> nn_indices (1); + std::vector<float> nn_sqr_dists (1); + + size_t numinliers = 0; + + for (size_t k = 0; k < transformedCloud.points.size(); ++k ) + { + if (mapTree->radiusSearch (transformedCloud.points[k], ICP_INLIER_DIST, nn_indices,nn_sqr_dists, 1) != 0) + numinliers += 1; + } + if (transformedCloud.points.size() > 0) + { + ROS_WARN("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD); + inlier_perc = (double) numinliers / (double) transformedCloud.points.size(); + } + } + pcl::toROSMsg (transformedCloud, cloud2transformed); we_have_a_scan_transformed = true; - double dist = sqrt((t.getOrigin().x() * t.getOrigin().x()) + (t.getOrigin().y() * t.getOrigin().y())); double angleDist = t.getRotation().getAngle(); - t = t * oldPose ; + btVector3 rotAxis = t.getRotation().getAxis(); + t = t * oldPose; + tf::StampedTransform base_after_icp; + if (!getTransform(base_after_icp, ODOM_FRAME, "base_link", ros::Time(0))) + { + ROS_ERROR("Did not get base pose at now"); + scan_callback_mutex.unlock(); + + return; + } + else + { + tf::Transform rel = base_at_laser.inverseTimes(base_after_icp); + ROS_INFO("relative motion of robot while doing icp: %fcm %fdeg", rel.getOrigin().length(), rel.getRotation().getAngle() * 180 / M_PI); + t= t * rel; + } + //ROS_INFO("dist %f angleDist %f",dist, angleDist); + ros::Time scan_time = scan_in->header.stamp; + ros::Duration scan_age = ros::Time::now() - scan_time; + //ROS_INFO("SCAN_AGE seems to be %f", scan_age.toSec()); + char msg_c_str[2048]; + sprintf(msg_c_str,"INLIERS %f (%f) scan_age %f (%f age_threshold) dist %f angleDist %f axis(%f %f %f) fitting %f (icp_fitness_threshold %f)",inlier_perc, ICP_INLIER_THRESHOLD, scan_age.toSec(), AGE_THRESHOLD ,dist, angleDist, rotAxis.x(), rotAxis.y(), rotAxis.z(),reg.getFitnessScore(), ICP_FITNESS_THRESHOLD ); + std_msgs::String strmsg; + strmsg.data = msg_c_str; - if ((actScan - lastTimeSent > 10) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD))) - if ( reg.getFitnessScore() <= ICP_FITNESS_THRESHOLD ) + //ROS_INFO("%s", msg_c_str); + + + //check if we want to accept this scan, if its older than 1 sec, drop it + if (!use_sim_time) + if (scan_age.toSec() > AGE_THRESHOLD) { - lastTimeSent = actScan; - geometry_msgs::PoseWithCovarianceStamped pose; - pose.header.frame_id = "map"; - pose.pose.pose.position.x = t.getOrigin().x(); - pose.pose.pose.position.y = t.getOrigin().y(); + ROS_ERROR("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD); + scan_callback_mutex.unlock(); - btQuaternion quat = t.getRotation(); - //quat.setRPY(0.0, 0.0, theta); - tf::quaternionTFToMsg(quat,pose.pose.pose.orientation); - float factorPos = 0.03; - float factorRot = 0.1; - pose.pose.covariance[6*0+0] = (0.5 * 0.5) * factorPos; - pose.pose.covariance[6*1+1] = (0.5 * 0.5) * factorPos; - pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot; - ROS_INFO("i %i converged %i SCORE: %f", i, reg.hasConverged (), reg.getFitnessScore() ); - ROS_INFO("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f [frame=%s]",dist, angleDist , pose.pose.pose.position.x , pose.pose.pose.position.y , pose.header.frame_id.c_str()); - pub_pose.publish(pose); + return; } + + + double cov = POSE_COVARIANCE_TRANS; + + //if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (angleDist < ANGLE_UPPER_THRESHOLD)) + // if ( reg.getFitnessScore() <= ICP_FITNESS_THRESHOLD ) + if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (inlier_perc > ICP_INLIER_THRESHOLD) && (angleDist < ANGLE_UPPER_THRESHOLD)) + { + lastTimeSent = actScan; + geometry_msgs::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.pose.pose.position.x = t.getOrigin().x(); + pose.pose.pose.position.y = t.getOrigin().y(); + + btQuaternion quat = t.getRotation(); + //quat.setRPY(0.0, 0.0, theta); + tf::quaternionTFToMsg(quat,pose.pose.pose.orientation); + float factorPos = 0.03; + float factorRot = 0.1; + pose.pose.covariance[6*0+0] = (cov * cov) * factorPos; + pose.pose.covariance[6*1+1] = (cov * cov) * factorPos; + pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot; + ROS_INFO("i %i converged %i SCORE: %f", i, reg.hasConverged (), reg.getFitnessScore() ); + ROS_INFO("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f [frame=%s]",dist, angleDist , pose.pose.pose.position.x , pose.pose.pose.position.y , pose.header.frame_id.c_str()); + pub_pose.publish(pose); + strmsg.data += " << SENT"; + } + + pub_info_.publish(strmsg); //ROS_DEBUG("map width %i height %i size %i, %s", myMapCloud.width, myMapCloud.height, (int)myMapCloud.points.size(), myMapCloud.header.frame_id.c_str()); //ROS_DEBUG("scan width %i height %i size %i, %s", myScanCloud.width, myScanCloud.height, (int)myScanCloud.points.size(), myScanCloud.header.frame_id.c_str()); } } - // Do something with cloud. + scan_callback_mutex.unlock(); } int main(int argc, char** argv) @@ -300,10 +487,15 @@ ros::NodeHandle nh_; nh = &nh_; + nh->param<std::string>("SnapMapICP/odom_frame", ODOM_FRAME, "/odom_combined"); + nh->param<std::string>("SnapMapICP/base_laser_frame", BASE_LASER_FRAME, "/base_laser_link"); + + projector_ = new laser_geometry::LaserProjection(); tf::TransformListener listener; listener_ = &listener; + pub_info_ = nh->advertise<std_msgs::String> ("SnapMapICP", 1); pub_output_ = nh->advertise<sensor_msgs::PointCloud2> ("map_points", 1); pub_output_scan = nh->advertise<sensor_msgs::PointCloud2> ("scan_points", 1); pub_output_scan_transformed = nh->advertise<sensor_msgs::PointCloud2> ("scan_points_transformed", 1); @@ -317,7 +509,7 @@ listener_->waitForTransform("/base_link", "/map", ros::Time(0), ros::Duration(30.0)); - listener_->waitForTransform("/base_laser_link", "/map", + listener_->waitForTransform(BASE_LASER_FRAME, "/map", ros::Time(0), ros::Duration(30.0)); while (ros::ok()) @@ -340,101 +532,3 @@ } } - - -/* -Full name: /Motors/EtherCAT Device (r_wrist_l_motor) -Component: EtherCAT Device (r_wrist_l_motor) -Hardware ID: 68-05005-01769 -Level: Warning -Message: Could not collect WG0X diagnostics; Could not collect diagnostics - -Configuration: good -Name: r_wrist_l_motor -Position: 31 -Product code: WG05 (6805005) Firmware Revision 1.21, PCB Revision F.02 -Robot: PR2 -Motor: Maxon 310009 -Serial Number: 68-05005-01769 -Nominal Current Scale: 0.000500 -Nominal Voltage Scale: 0.006250 -HW Max Current: 3.250000 -SW Max Current: 1.720000 -Speed Constant: 178.000000 -Resistance: 2.520000 -Motor Torque Constant: 0.053800 -Pulses Per Revolution: 1200 -Encoder Reduction: -1.000000 -Status Checksum Error Count: 0 -Safety Disable Status: ENABLED (00) -Safety Disable Status Hold: ENABLED (00) -Safety Disable Count: 6 -Undervoltage Count: 5 -Over Current Count: 0 -Board Over Temp Count: 0 -Bridge Over Temp Count: 0 -Operate Disable Count: 1 -Watchdog Disable Count: 1 -MBX Command IRQ Count: 181 -PDI Timeout Error Count: 0 -PDI Checksum Error Count: 0 -Supply Current: 0.051398 -Configured Offset A: 0.022000 -Configured Offset B: -0.006000 -Mailbox Write Errors: 0 -Mailbox Read Errors: 0 -Mailbox Retries: 0 -Mailbox Retry Errors: 0 -Calibration Offset: -198.797365 -Calibration Status: Calibrated by controller -Watchdog Limit: 100ms -Mode: ENABLE, CURRENT -Digital out: 254 -Programmed pwm value: -291 -Programmed current: -0.197000 -Measured current: -0.194500 -Timestamp: 3546451304 -Encoder count: -38034 -Encoder index pos: -38034 -Num encoder_errors: 0 -Encoder status: 15 -Calibration reading: 15 -Last calibration rising edge: -43040 -Last calibration falling edge: -18976 -Board temperature: 44.250000 -Max board temperature: 44.375000 -Bridge temperature: 50.687500 -Max bridge temperature: 50.812500 -Supply voltage: 70.787498 -Motor voltage: -1.125000 -Current Loop Kp: 16 -Current Loop Ki: 4 -Motor Voltage Error %: 10.172653 -Max Motor Voltage Error %: 34.376442 -Abs Filtered Voltage Error %: 8.273659 -Max Abs Filtered Voltage Error %: 15.651860 -Current Error: 0.000720 -Max Current Error: 0.017975 -Abs Filtered Current Error: 0.000570 -Max Abs Filtered Current Error: 0.003362 -Motor Resistance Estimate: 4.495118 -# Published traces: 5 -Packet count: 48769 -Drops: 0 -Consecutive Drops: 0 -Max Consecutive Drops: 0 -Reset detected: No -Valid: No -EPU Errors: 0 -PDI Errors: 0 -Status Port 0: Has Link, Open, Has Comm -RX Error Port 0: 0 -Forwarded RX Error Port 0: 0 -Invalid Frame Port 0: 0 -Lost Link Port 0: 0 -Status Port 1: Has Link, Open, Has Comm -RX Error Port 1: 0 -Forwarded RX Error Port 1: 0 -Invalid Frame Port 1: 0 -Lost Link Port 1: 0 -*/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-02-09 10:13:53
|
Revision: 990 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=990&view=rev Author: moesenle Date: 2012-02-09 10:13:47 +0000 (Thu, 09 Feb 2012) Log Message: ----------- Moved cram_pr2 and cram_physics to github. Modified Paths: -------------- rosinstall/tum-ros-pkg.rosinstall Modified: rosinstall/tum-ros-pkg.rosinstall =================================================================== --- rosinstall/tum-ros-pkg.rosinstall 2012-02-05 18:32:38 UTC (rev 989) +++ rosinstall/tum-ros-pkg.rosinstall 2012-02-09 10:13:47 UTC (rev 990) @@ -15,7 +15,7 @@ uri: http://code.in.tum.de/git/mapping.git - git: local-name: cram_physics - uri: git://code.in.tum.de/git/cram-physics.git + uri: https://github.com/moesenle/cram_physics.git - git: local-name: cram_pl uri: http://code.in.tum.de/git/cram-pl.git @@ -27,7 +27,7 @@ uri: http://code.in.tum.de/git/cram-rosie.git - git: local-name: cram_pr2 - uri: http://code.in.tum.de/git/cram-pr2.git + uri: https://github.com/moesenle/cram_pr2.git - git: local-name: ias_common uri: http://code.in.tum.de/git/ias-common.git This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-02-05 18:32:45
|
Revision: 989 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=989&view=rev Author: moesenle Date: 2012-02-05 18:32:38 +0000 (Sun, 05 Feb 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@cs...> Date: Sun Feb 5 19:32:04 2012 +0100 commit 0d6623d44c2cb9056ee62606f63b79be374f2dfe Author: Dejan Pangercic <dej...@cs...> Date: Sun Feb 5 19:31:38 2012 +0100 refactored the code commit 2b90812f874db614252cc411ea9f6581e05cc527 Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 17:56:00 2012 +0100 public export git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/launch/barcode_reader.launch perception/zbar_barcode_reader_node/rosdep.yaml perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp Modified: perception/zbar_barcode_reader_node/launch/barcode_reader.launch =================================================================== --- perception/zbar_barcode_reader_node/launch/barcode_reader.launch 2012-01-31 16:56:47 UTC (rev 988) +++ perception/zbar_barcode_reader_node/launch/barcode_reader.launch 2012-02-05 18:32:38 UTC (rev 989) @@ -1,5 +1,12 @@ <launch> - <node pkg="zbar_barcode_reader_node" type="zbar_barcode_reader_node" name="barcode_reader_node" output="screen" respawn="true"> + <node pkg="zbar_barcode_reader_node" type="zbar_barcode_reader_node" name="barcode_reader_node" output="screen" respawn="false"> <param name="input_image_topic" type="string" value="/image_raw" /> + <param name="link1" type="string" value="http://www.barcoo.com/api/get_product_complete?pi=" /> + <param name="link2" type="string" value="&pins=ean&format=xml&source=ias-tum" /> + <param name="tag1" type="string" value="answer" /> + <param name="tag2" type="string" value="picture_high" /> + <param name="tag3" type="string" value="picture_low" /> + <param name="image_pattern" type="string" value="<meta property="og:image" content="" /> + </node> </launch> Modified: perception/zbar_barcode_reader_node/rosdep.yaml =================================================================== --- perception/zbar_barcode_reader_node/rosdep.yaml 2012-01-31 16:56:47 UTC (rev 988) +++ perception/zbar_barcode_reader_node/rosdep.yaml 2012-02-05 18:32:38 UTC (rev 989) @@ -1,7 +1,11 @@ libcurl: ubuntu: 11.04: libcurl3 + oneiric: libcurl3 libtinyxml: ubuntu: 11.04: libtinyxml-dev + oneiric: libtinyxml-dev + + Modified: perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-01-31 16:56:47 UTC (rev 988) +++ perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-02-05 18:32:38 UTC (rev 989) @@ -22,6 +22,13 @@ using namespace zbar; class BarcodeReaderNode { + std:: string link1_;// = "http://www.barcoo.com/api/get_product_complete?pi="; + std:: string link2_;// = "&pins=ean&format=xml&source=ias-tum"; + std:: string tag1_;// = "answer"; + std:: string tag2_;// = "picture_high"; + std:: string tag3_;// = "picture_low"; + std:: string pattern_;// = "<meta property=\"og:image\" content=\""; + TiXmlDocument doc; public: struct memoryStruct { @@ -39,8 +46,19 @@ BarcodeReaderNode(ros::NodeHandle &n) : n_(n), it_(n_) { - n_.param ("input_image_topic", input_image_topic_, std::string("/stereo/left/image_rect")); + //link1 = "http://www.barcoo.com/api/get_product_complete?pi="; + //link2 = "&pins=ean&format=xml&source=ias-tum"; + //tag1 = "answer"; + //tag2 = "picture_high"; + //tag3 = "picture_low"; + + n_.param ("input_image_topic", input_image_topic_, std::string("/image_raw")); n_.param ("outout_barcode_topic", output_barcode_topic_, std::string("barcode")); + n_.param ("link1", link1_, std::string("http://www.barcoo.com/api/get_product_complete?pi=")); + n_.param ("link2", link2_, std::string("&pins=ean&format=xml&source=ias-tum")); + n_.param ("tag2", tag2_, std::string("picture_high")); + n_.param ("tag3", tag3_, std::string("picture_low")); + n_.param ("image_pattern", pattern_, std::string("<meta property=\"og:image\" content=\"")); image_sub_ = it_.subscribe( input_image_topic_, 1, &BarcodeReaderNode::imageCallback, this); barcode_pub_ = @@ -67,7 +85,7 @@ { UserData *userdata = reinterpret_cast<UserData *>(data); struct memoryStruct *mem = userdata->memory; - BarcodeReaderNode *self = userdata->self; + //BarcodeReaderNode *self = userdata->self; size_t realsize = size * nmemb; mem->memory = (char *) @@ -100,11 +118,10 @@ return result; } - int getImage(std::string & image) + int getImage(std::string image, cv::Mat * imgTmp) { CURL *curl; // CURL objects CURLcode res; - cv::Mat imgTmp; // image object memoryStruct buffer; // memory buffer UserData userdata(&buffer, this); @@ -129,16 +146,7 @@ // get the image from the specified URL res = curl_easy_perform(curl); // decode memory buffer using OpenCV - imgTmp = cv::imdecode(cv::Mat(1, buffer.size, CV_8UC1, buffer.memory), CV_LOAD_IMAGE_UNCHANGED); - // display image (if we got / decoded it correctly) - - - if (!(imgTmp.empty())) - { - imshow("Barcoo img", imgTmp); - } - cv::waitKey(3); - + *imgTmp = cv::imdecode(cv::Mat(1, buffer.size, CV_8UC1, buffer.memory), CV_LOAD_IMAGE_UNCHANGED); // always cleanup curl_easy_cleanup(curl); @@ -147,52 +155,60 @@ return 1; } - void findElement( TiXmlNode* pParent, std::string & picture) + void findElement( TiXmlNode* pParent, std::string & picture, std:: string tag) { - if ( !pParent ) return; + //if ( !pParent ) return; TiXmlNode* pChild; - //std::string picture ("Picture Not Found"); std::string pstring = pParent->Value(); - // std::cerr << "pstring: " << pstring << std::endl; - size_t found=pstring.find("picture_high"); + + //Search for tag + size_t found=pstring.find(tag); if (found!=std::string::npos) - { - ROS_INFO_STREAM("First child: " << pParent->FirstChild()->Value()); - picture = pParent->FirstChild()->Value(); - return; - } - size_t found2=pstring.find("picture_low"); - if (found2!=std::string::npos && picture == "") - { - ROS_INFO_STREAM("First child: " << pParent->FirstChild()->Value()); - picture = pParent->FirstChild()->Value(); - return; - } - for ( pChild = pParent->FirstChild(); pChild != 0; pChild = pChild->NextSibling()) - { - findElement(pChild, picture); - // std::cerr << "in for " << std::endl; - } + { + ROS_INFO_STREAM("First child: " << pParent->FirstChild()->Value()); + picture = pParent->FirstChild()->Value(); + return; + } + + for( pChild = pParent->FirstChild(); pChild != 0; pChild = pChild->NextSibling()) + { + findElement(pChild, picture,tag); + } } - int visBarcooPicture(std::string buffer) + int getPictureLink(std::string buffer,std::string & picture) { - // std::cerr << "buffer: " << buffer << std::endl; - TiXmlDocument doc; doc.Parse((const char*)buffer.c_str(), 0, TIXML_ENCODING_UTF8); - std::string picture; - findElement (&doc, picture); + + //Check if there is a result + std:: string result; + cout << "ohhh my goooooood\n\n"; + findElement(&doc,result,tag1_); + if(result.compare("0") == 0) //This condition checks if there is a result in the XML file if not returns 0 + { + cout << "heloooooo barcode does not correspond to any object in the barcoo database"; + return 0; + } + else + cout << "ELSE ELSE ELSE\n"; + + //Search for first tag + findElement (&doc, picture,tag2_); + + if(picture == "") + findElement (&doc, picture,tag3_); //Search for second tag + ROS_INFO_STREAM ("Picture link: " << picture); if (picture == "") return -1; - getImage (picture); + return 1; } - int callBarcoo(std::string bar_code, std::string & buffer) + int getBarcooXML(std::string bar_code, std::string & buffer) { char errorBuffer[CURL_ERROR_SIZE]; // Our curl objects @@ -200,7 +216,7 @@ CURLcode result; // Create our curl handle curl = curl_easy_init(); - std::string full_url = "http://www.barcoo.com/api/get_product_complete?pi=" + bar_code + "&pins=ean&format=xml&source=ias-tum"; + std::string full_url = link1_ + bar_code + link2_; ROS_INFO_STREAM("full_url: " << full_url); if (curl) @@ -236,6 +252,45 @@ return -1; } + int getHTMLpage(std::string url, std::string & buffer) + { + char errorBuffer[CURL_ERROR_SIZE]; + // Our curl objects + CURL *curl; + CURLcode result; + // Create our curl handle + curl = curl_easy_init(); + if (curl) + { + // Now set up all of the curl options + curl_easy_setopt(curl, CURLOPT_ERRORBUFFER, errorBuffer); + curl_easy_setopt(curl, CURLOPT_URL, url.c_str()); + curl_easy_setopt(curl, CURLOPT_HEADER, 0); + curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, &BarcodeReaderNode::writer); + // curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, writer); + curl_easy_setopt(curl, CURLOPT_WRITEDATA, &buffer); + + // Attempt to retrieve the remote page + result = curl_easy_perform(curl); + + // Always cleanup + curl_easy_cleanup(curl); + + // Did we succeed? + if (result == CURLE_OK) + { + + return 1; + } + else + { + return -1; + } + } + return -1; + } + void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr) { @@ -288,26 +343,70 @@ barcode_pub_.publish(msg); } if (n == 0) - { - ROS_WARN("Barcode not found"); - return; - } + { + ROS_WARN("Barcode not found"); + return; + } if (n < 0) - { - ROS_ERROR("Error occured while finding barcode"); - return; - } + { + ROS_ERROR("Error occured while finding barcode"); + return; + } std::string buffer; - if (callBarcoo(ss.str(), buffer) == 1) - { - // std::cerr << "buffer after callBarcoo " << buffer << std::endl; - visBarcooPicture (buffer); - } + //Get xml file from barcoo database + int res = getBarcooXML(ss.str(), buffer); + if(res != 1) + return; + // std::cerr << "buffer after callBarcoo " << buffer << std::endl; + //Search for info in the xml file + std::string pictureLink; + res = getPictureLink (buffer,pictureLink); + + + if(res == 0) //If barcode does not exist in the database return + { + cout << "Barcode does not exist in the barcoo datbase\n"; + return; + } + if(res == -1) //This condition is true when the image link is not given + { + //Look for an image in the HTML file + + //First find the link to the barcoo website of the file + std::string htmlPage; + std::string htmlLink; + findElement(&doc,htmlLink,"back_link"); + cout << "html link: "+htmlLink+"\n"; + //Download HTML page + getHTMLpage(htmlLink,htmlPage); + std::string ss = pattern_; + long position = htmlPage.find(ss, ss.length()) + ss.length(); + + std::string link; + while(htmlPage.at(position) !='"') + { + link += htmlPage.at(position); + position++; + } + pictureLink = link; + + } + //Get Image + if(!pictureLink.empty()) + { + cv::Mat imgTmp; // image object + getImage (pictureLink ,& imgTmp); + // display image + if (!(imgTmp.empty())) + { + imshow("Barcoo img", imgTmp); + } + } + cv::waitKey(3); // clean up image.set_data(NULL, 0); - } protected: This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-01-31 16:56:54
|
Revision: 988 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=988&view=rev Author: moesenle Date: 2012-01-31 16:56:47 +0000 (Tue, 31 Jan 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 17:56:00 2012 +0100 commit 18a0d38c68c6c7687b2dc686099d920b40fa5f8d Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 17:55:38 2012 +0100 no need to save an xml commit 1f4fc5e466ea6b71c55184c96db00c0861f6287a Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 17:55:00 2012 +0100 public export git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp Modified: perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-01-31 16:56:40 UTC (rev 987) +++ perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-01-31 16:56:47 UTC (rev 988) @@ -182,7 +182,6 @@ // std::cerr << "buffer: " << buffer << std::endl; TiXmlDocument doc; doc.Parse((const char*)buffer.c_str(), 0, TIXML_ENCODING_UTF8); - doc.SaveFile ("text.xml"); std::string picture; findElement (&doc, picture); ROS_INFO_STREAM ("Picture link: " << picture); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-01-31 16:56:49
|
Revision: 987 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=987&view=rev Author: moesenle Date: 2012-01-31 16:56:40 +0000 (Tue, 31 Jan 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 17:55:00 2012 +0100 commit 0b6c0c9cb2a8e0f40e1b195cc0ae1472185c591a Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 17:54:39 2012 +0100 got rid of static functions (thx Lorenz) commit 4810db7739748ed8a5f4750d6a992fbaa4fb6a6e Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 13:04:23 2012 +0100 different default topic commit e73893c803108e9e9dc500fa1e77667da40da120 Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 12:07:05 2012 +0100 public export git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/launch/barcode_reader.launch perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp Modified: perception/zbar_barcode_reader_node/launch/barcode_reader.launch =================================================================== --- perception/zbar_barcode_reader_node/launch/barcode_reader.launch 2012-01-31 11:07:48 UTC (rev 986) +++ perception/zbar_barcode_reader_node/launch/barcode_reader.launch 2012-01-31 16:56:40 UTC (rev 987) @@ -1,5 +1,5 @@ <launch> <node pkg="zbar_barcode_reader_node" type="zbar_barcode_reader_node" name="barcode_reader_node" output="screen" respawn="true"> - <param name="input_image_topic" type="string" value="/image_topic_2" /> + <param name="input_image_topic" type="string" value="/image_raw" /> </node> </launch> Modified: perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-01-31 11:07:48 UTC (rev 986) +++ perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-01-31 16:56:40 UTC (rev 987) @@ -21,64 +21,21 @@ using namespace std; using namespace zbar; - static int writer(char *data, size_t size, size_t nmemb, - std::string *buffer) - { - // What we will return - int result = 0; - - // Is there anything in the buffer? - if (buffer != NULL) - { - // Append the data to the buffer - buffer->append(data, size * nmemb); - - // How much did we write? - result = size * nmemb; - } - - return result; - } - -static void* CURL_realloc(void *ptr, size_t size) -{ - /* There might be a realloc() out there that doesn't like reallocing - NULL pointers, so we take care of it here */ - if(ptr) - return realloc(ptr, size); - else - return malloc(size); -} - -struct memoryStruct { - char *memory; - size_t size; -}; - - size_t WriteMemoryCallback - (void *ptr, size_t size, size_t nmemb, void *data) - { - size_t realsize = size * nmemb; - struct memoryStruct *mem = (struct memoryStruct *)data; - - mem->memory = (char *) - CURL_realloc(mem->memory, mem->size + realsize + 1); - if (mem->memory) { - memcpy(&(mem->memory[mem->size]), ptr, realsize); - mem->size += realsize; - mem->memory[mem->size] = 0; - } - return realsize; - } - - class BarcodeReaderNode { - // struct memoryStruct { - // char *memory; - // size_t size; - // }; public: + struct memoryStruct { + char *memory; + size_t size; + }; + + struct UserData { + memoryStruct *memory; + BarcodeReaderNode *self; + UserData(memoryStruct *memory, BarcodeReaderNode *self) + : memory(memory), self(self) {} + }; + BarcodeReaderNode(ros::NodeHandle &n) : n_(n), it_(n_) { @@ -86,63 +43,70 @@ n_.param ("outout_barcode_topic", output_barcode_topic_, std::string("barcode")); image_sub_ = it_.subscribe( input_image_topic_, 1, &BarcodeReaderNode::imageCallback, this); - barcode_pub_ = n_.advertise<std_msgs::String>(output_barcode_topic_, 1); + barcode_pub_ = + n_.advertise<std_msgs::String>(output_barcode_topic_, 1); + cv::namedWindow ("Barcoo img"); } ~BarcodeReaderNode() { - + cv::destroyAllWindows(); } - // void* CURL_realloc(void *ptr, size_t size) - // { - // /* There might be a realloc() out there that doesn't like reallocing - // NULL pointers, so we take care of it here */ - // if(ptr) - // return realloc(ptr, size); - // else - // return malloc(size); - // } + static void* CURL_realloc(void *ptr, size_t size) + { + /* There might be a realloc() out there that doesn't like reallocing + NULL pointers, so we take care of it here */ + if(ptr) + return realloc(ptr, size); + else + return malloc(size); + } - // size_t WriteMemoryCallback - // (void *ptr, size_t size, size_t nmemb, void *data) - // { - // size_t realsize = size * nmemb; - // struct memoryStruct *mem = (struct memoryStruct *)data; + static size_t WriteMemoryCallback + (void *ptr, size_t size, size_t nmemb, void *data) + { + UserData *userdata = reinterpret_cast<UserData *>(data); + struct memoryStruct *mem = userdata->memory; + BarcodeReaderNode *self = userdata->self; + size_t realsize = size * nmemb; - // mem->memory = (char *) - // CURL_realloc(mem->memory, mem->size + realsize + 1); - // if (mem->memory) { - // memcpy(&(mem->memory[mem->size]), ptr, realsize); - // mem->size += realsize; - // mem->memory[mem->size] = 0; - // } - // return realsize; - // } + mem->memory = (char *) + CURL_realloc(mem->memory, mem->size + realsize + 1); + if (mem->memory) { + memcpy(&(mem->memory[mem->size]), ptr, realsize); + mem->size += realsize; + mem->memory[mem->size] = 0; + } + return realsize; + } + // This is the writer call back function used by curl - // int writer(char *data, size_t size, size_t nmemb, - // std::string *buffer) - // { - // // What we will return - // int result = 0; - - // // Is there anything in the buffer? - // if (buffer != NULL) - // { - // // Append the data to the buffer - // buffer->append(data, size * nmemb); - - // // How much did we write? - // result = size * nmemb; - // } - - // return result; - // } + static int writer(char *data, size_t size, size_t nmemb, + std::string *buffer) + { + // What we will return + int result = 0; + + // Is there anything in the buffer? + if (buffer != NULL) + { + // Append the data to the buffer + buffer->append(data, size * nmemb); + + // How much did we write? + result = size * nmemb; + } + + return result; + } + int getImage(std::string & image) { CURL *curl; // CURL objects CURLcode res; cv::Mat imgTmp; // image object memoryStruct buffer; // memory buffer + UserData userdata(&buffer, this); curl = curl_easy_init(); // init CURL library object/structure @@ -159,8 +123,8 @@ // tell libcurl where to write the image (to a dynamic memory buffer) - curl_easy_setopt(curl,CURLOPT_WRITEFUNCTION, WriteMemoryCallback); - curl_easy_setopt(curl,CURLOPT_WRITEDATA, (void *) &buffer); + curl_easy_setopt(curl,CURLOPT_WRITEFUNCTION, &BarcodeReaderNode::WriteMemoryCallback); + curl_easy_setopt(curl,CURLOPT_WRITEDATA, (void *) &userdata); // get the image from the specified URL res = curl_easy_perform(curl); @@ -171,7 +135,7 @@ if (!(imgTmp.empty())) { - imshow("Image from URL", imgTmp); + imshow("Barcoo img", imgTmp); } cv::waitKey(3); @@ -247,8 +211,8 @@ curl_easy_setopt(curl, CURLOPT_URL, full_url.c_str()); curl_easy_setopt(curl, CURLOPT_HEADER, 0); curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1); - //curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, &BarcodeReaderNode::writer); - curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, writer); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, &BarcodeReaderNode::writer); + // curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, writer); curl_easy_setopt(curl, CURLOPT_WRITEDATA, &buffer); // Attempt to retrieve the remote page This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-01-31 11:07:55
|
Revision: 986 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=986&view=rev Author: moesenle Date: 2012-01-31 11:07:48 +0000 (Tue, 31 Jan 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 12:07:05 2012 +0100 commit 05ca8af0ca8c6b8e96876789e20ddceac4db4ecc Author: Dejan Pangercic <dej...@gm...> Date: Tue Jan 31 12:06:34 2012 +0100 added system deps commit ac04cfa29420148be13c98195eb1ea423b406457 Author: Dejan Pangercic <dej...@cs...> Date: Mon Jan 30 21:53:49 2012 +0100 not needed filed anymore commit e0ae46178d3e1ce6787ba2bb0d5924f142d4fc34 Author: Dejan Pangercic <dej...@cs...> Date: Mon Jan 30 21:49:26 2012 +0100 public export commit 694aac589283166617448c925b62fea7e506f343 Author: Dejan Pangercic <dej...@cs...> Date: Mon Jan 30 21:48:26 2012 +0100 object recognition using barcoo working, major refactoring of the barcode_reader_node.zpp needed git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/CMakeLists.txt perception/zbar_barcode_reader_node/manifest.xml Added Paths: ----------- perception/zbar_barcode_reader_node/rosdep.yaml Removed Paths: ------------- perception/zbar_barcode_reader_node/src/scan_image.cpp Modified: perception/zbar_barcode_reader_node/CMakeLists.txt =================================================================== --- perception/zbar_barcode_reader_node/CMakeLists.txt 2012-01-30 20:53:08 UTC (rev 985) +++ perception/zbar_barcode_reader_node/CMakeLists.txt 2012-01-31 11:07:48 UTC (rev 986) @@ -31,9 +31,6 @@ rosbuild_add_executable(zbar_barcode_reader src/zbar_barcode_reader.c) target_link_libraries(zbar_barcode_reader MagickWand MagickCore) -rosbuild_add_executable(zbar_barcode_reader_cpp src/scan_image.cpp) -target_link_libraries(zbar_barcode_reader_cpp MagickWand MagickCore Magick++ curl) - rosbuild_add_executable(zbar_barcode_reader_node src/zbar_barcode_reader_node.cpp) target_link_libraries(zbar_barcode_reader_node Magick++ curl) Modified: perception/zbar_barcode_reader_node/manifest.xml =================================================================== --- perception/zbar_barcode_reader_node/manifest.xml 2012-01-30 20:53:08 UTC (rev 985) +++ perception/zbar_barcode_reader_node/manifest.xml 2012-01-31 11:07:48 UTC (rev 986) @@ -13,7 +13,8 @@ <depend package="cv_bridge"/> <depend package="image_transport"/> <depend package="sensor_msgs"/> - + <rosdep name="libcurl"/> + <rosdep name="libtinyxml"/> </package> Added: perception/zbar_barcode_reader_node/rosdep.yaml =================================================================== --- perception/zbar_barcode_reader_node/rosdep.yaml (rev 0) +++ perception/zbar_barcode_reader_node/rosdep.yaml 2012-01-31 11:07:48 UTC (rev 986) @@ -0,0 +1,7 @@ +libcurl: + ubuntu: + 11.04: libcurl3 + +libtinyxml: + ubuntu: + 11.04: libtinyxml-dev Deleted: perception/zbar_barcode_reader_node/src/scan_image.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/scan_image.cpp 2012-01-30 20:53:08 UTC (rev 985) +++ perception/zbar_barcode_reader_node/src/scan_image.cpp 2012-01-31 11:07:48 UTC (rev 986) @@ -1,54 +0,0 @@ -#include <iostream> -#include <Magick++.h> -#include <zbar.h> -#define STR(s) #s - -using namespace std; -using namespace zbar; - -int main (int argc, char **argv) -{ - if(argc < 2) return(1); - -#ifdef MAGICK_HOME - // http://www.imagemagick.org/Magick++/ - // under Windows it is necessary to initialize the ImageMagick - // library prior to using the Magick++ library - Magick::InitializeMagick(MAGICK_HOME); -#endif - - // create a reader - ImageScanner scanner; - - // configure the reader - scanner.set_config(ZBAR_NONE, ZBAR_CFG_ENABLE, 1); - - // obtain image data - Magick::Image magick(argv[1]); // read an image file - int width = magick.columns(); // extract dimensions - int height = magick.rows(); - Magick::Blob blob; // extract the raw data - magick.modifyImage(); - magick.write(&blob, "GRAY", 8); - const void *raw = blob.data(); - - // wrap image data - Image image(width, height, "Y800", raw, width * height); - - // scan the image for barcodes - int n = scanner.scan(image); - - // extract results - for(Image::SymbolIterator symbol = image.symbol_begin(); - symbol != image.symbol_end(); - ++symbol) { - // do something useful with results - cout << "decoded " << symbol->get_type_name() - << " symbol \"" << symbol->get_data() << '"' << endl; - } - - // clean up - image.set_data(NULL, 0); - - return(0); -} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-01-30 20:53:15
|
Revision: 985 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=985&view=rev Author: moesenle Date: 2012-01-30 20:53:08 +0000 (Mon, 30 Jan 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@cs...> Date: Mon Jan 30 21:49:26 2012 +0100 commit 7d2243e92e66feb8b1f48eab599384708ffa3da7 Author: Dejan Pangercic <dej...@cs...> Date: Mon Jan 30 21:48:26 2012 +0100 object recognition using barcoo working, major refactoring of the barcode_reader_node.zpp needed commit 73c93f2f13c5445cfc3955773a323c5e98deada8 Author: Dejan Pangercic <dej...@cs...> Date: Mon Jan 30 21:47:14 2012 +0100 examples for tinyxml and loading of images with curl and opencv commit c358c7210ee1c53d032b34118fb84b15c35b9c68 Author: Dejan Pangercic <dej...@cs...> Date: Sun Aug 29 21:59:21 2010 +0200 created a node for barcode reading, input: image output: type,value exchanged c for c++ interface tested TODO: find a n..... that will port to cop:-) commit 0fd52914acf001cc75df60a84dffc0395a2e769b Author: Dejan Pangercic <dej...@cs...> Date: Sun Aug 29 21:24:21 2010 +0200 c++ implementation of zbar git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/CMakeLists.txt perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp Added Paths: ----------- perception/zbar_barcode_reader_node/src/curl_opencv.cpp perception/zbar_barcode_reader_node/src/tinyxml.cpp Modified: perception/zbar_barcode_reader_node/CMakeLists.txt =================================================================== --- perception/zbar_barcode_reader_node/CMakeLists.txt 2012-01-27 18:36:55 UTC (rev 984) +++ perception/zbar_barcode_reader_node/CMakeLists.txt 2012-01-30 20:53:08 UTC (rev 985) @@ -32,7 +32,13 @@ target_link_libraries(zbar_barcode_reader MagickWand MagickCore) rosbuild_add_executable(zbar_barcode_reader_cpp src/scan_image.cpp) -target_link_libraries(zbar_barcode_reader_cpp MagickWand MagickCore Magick++) +target_link_libraries(zbar_barcode_reader_cpp MagickWand MagickCore Magick++ curl) rosbuild_add_executable(zbar_barcode_reader_node src/zbar_barcode_reader_node.cpp) -target_link_libraries(zbar_barcode_reader_node Magick++) \ No newline at end of file +target_link_libraries(zbar_barcode_reader_node Magick++ curl) + +# rosbuild_add_executable(test_tinyxml src/tinyxml.cpp) +# target_link_libraries(test_tinyxml tinyxml) + +# rosbuild_add_executable(curl_opencv src/curl_opencv.cpp) +# target_link_libraries(curl_opencv curl) Added: perception/zbar_barcode_reader_node/src/curl_opencv.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/curl_opencv.cpp (rev 0) +++ perception/zbar_barcode_reader_node/src/curl_opencv.cpp 2012-01-30 20:53:08 UTC (rev 985) @@ -0,0 +1,95 @@ +#include <stdio.h> +#include <curl/curl.h> + +#include <cv.h> +#include <highgui.h> +using namespace cv; + +// ***************************************************************************** + +struct memoryStruct { + char *memory; + size_t size; +}; + +static void* CURL_realloc(void *ptr, size_t size) +{ + /* There might be a realloc() out there that doesn't like reallocing + NULL pointers, so we take care of it here */ + if(ptr) + return realloc(ptr, size); + else + return malloc(size); +} + +size_t WriteMemoryCallback +(void *ptr, size_t size, size_t nmemb, void *data) +{ + size_t realsize = size * nmemb; + struct memoryStruct *mem = (struct memoryStruct *)data; + + mem->memory = (char *) + CURL_realloc(mem->memory, mem->size + realsize + 1); + if (mem->memory) { + memcpy(&(mem->memory[mem->size]), ptr, realsize); + mem->size += realsize; + mem->memory[mem->size] = 0; + } + return realsize; +} + +// ***************************************************************************** + +int main(void) +{ + CURL *curl; // CURL objects + CURLcode res; + cv::Mat imgTmp; // image object + memoryStruct buffer; // memory buffer + + curl = curl_easy_init(); // init CURL library object/structure + + if(curl) { + + // set up the write to memory buffer + // (buffer starts off empty) + + buffer.memory = NULL; + buffer.size = 0; + + // (N.B. check this URL still works in browser in case image has moved) + + curl_easy_setopt(curl, CURLOPT_URL, "http://www.froodies.de/FroodiesImages/images/products/large/29278.jpg"); + curl_easy_setopt(curl, CURLOPT_VERBOSE, 1); // tell us what is happening + + // tell libcurl where to write the image (to a dynamic memory buffer) + + curl_easy_setopt(curl,CURLOPT_WRITEFUNCTION, WriteMemoryCallback); + curl_easy_setopt(curl,CURLOPT_WRITEDATA, (void *) &buffer); + + // get the image from the specified URL + + res = curl_easy_perform(curl); + + // decode memory buffer using OpenCV + + imgTmp = cv::imdecode(cv::Mat(1, buffer.size, CV_8UC1, buffer.memory), CV_LOAD_IMAGE_UNCHANGED); + + // display image (if we got / decoded it correctly) + + namedWindow("Image from URL", CV_WINDOW_AUTOSIZE); + if (!(imgTmp.empty())) + { + imshow("Image from URL", imgTmp); + } + waitKey(0); + + + // always cleanup + + curl_easy_cleanup(curl); + free(buffer.memory); + + } + return 0; +} Property changes on: perception/zbar_barcode_reader_node/src/curl_opencv.cpp ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Added: perception/zbar_barcode_reader_node/src/tinyxml.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/tinyxml.cpp (rev 0) +++ perception/zbar_barcode_reader_node/src/tinyxml.cpp 2012-01-30 20:53:08 UTC (rev 985) @@ -0,0 +1,146 @@ +// tutorial demo program +//#include "stdafx.h" +#include "tinyxml.h" + +// ---------------------------------------------------------------------- +// STDOUT dump and indenting utility functions +// ---------------------------------------------------------------------- +const unsigned int NUM_INDENTS_PER_SPACE=2; + +const char * getIndent( unsigned int numIndents ) +{ + static const char * pINDENT=" + "; + static const unsigned int LENGTH=strlen( pINDENT ); + unsigned int n=numIndents*NUM_INDENTS_PER_SPACE; + if ( n > LENGTH ) n = LENGTH; + + return &pINDENT[ LENGTH-n ]; +} + +// same as getIndent but no "+" at the end +const char * getIndentAlt( unsigned int numIndents ) +{ + static const char * pINDENT=" "; + static const unsigned int LENGTH=strlen( pINDENT ); + unsigned int n=numIndents*NUM_INDENTS_PER_SPACE; + if ( n > LENGTH ) n = LENGTH; + + return &pINDENT[ LENGTH-n ]; +} + +int dump_attribs_to_stdout(TiXmlElement* pElement, unsigned int indent) +{ + if ( !pElement ) return 0; + + TiXmlAttribute* pAttrib=pElement->FirstAttribute(); + int i=0; + int ival; + double dval; + const char* pIndent=getIndent(indent); + printf("\n"); + while (pAttrib) + { + printf( "%s%s: value=[%s]", pIndent, pAttrib->Name(), pAttrib->Value()); + + if (pAttrib->QueryIntValue(&ival)==TIXML_SUCCESS) printf( " int=%d", ival); + if (pAttrib->QueryDoubleValue(&dval)==TIXML_SUCCESS) printf( " d=%1.1f", dval); + printf( "\n" ); + i++; + pAttrib=pAttrib->Next(); + } + return i; +} + +void dump_to_stdout( TiXmlNode* pParent, unsigned int indent = 0 ) +{ + if ( !pParent ) return; + + TiXmlNode* pChild; + TiXmlText* pText; + int t = pParent->Type(); + //printf( "%s", getIndent(indent)); + int num; + + //switch ( t ) + //{ + // case TiXmlNode::DOCUMENT: + // printf( "Document" ); + // break; + + //case TiXmlNode::ELEMENT: + //case "picture_high": + // if (pParent->Value() == "picture_high") + // { + std::string pstring = pParent->Value(); + // std::cerr << "pstring: " << pstring << std::endl; + size_t found=pstring.rfind("picture_high"); + if (found!=std::string::npos) + { + std::cerr << "First child: " << pParent->FirstChild()->Value() << std::endl; + } + // std::cerr << "picture_high: " << int(found) << std::endl; + // exit(0); + // } + // printf( "Element value [%s]", pParent->Value() ); + // num=dump_attribs_to_stdout(pParent->ToElement(), indent+1); + // switch(num) + // { + // case 0: printf( " (No attributes)"); break; + // case 1: printf( "%s1 attribute", getIndentAlt(indent)); break; + // default: printf( "%s%d attributes", getIndentAlt(indent), num); break; + // } + // break; + + // case TiXmlNode::COMMENT: + // printf( "Comment: [%s]", pParent->Value()); + // break; + + // case TiXmlNode::UNKNOWN: + // printf( "Unknown" ); + // break; + + // case TiXmlNode::TEXT: + // pText = pParent->ToText(); + // printf( "Text: [%s]", pText->Value() ); + // break; + + // case TiXmlNode::DECLARATION: + // printf( "Declaration" ); + // break; + // default: + // break; + //} + //printf( "\n" ); + for ( pChild = pParent->FirstChild(); pChild != 0; pChild = pChild->NextSibling()) + { + dump_to_stdout( pChild, indent+1 ); + } +} + +// load the named file and dump its structure to STDOUT +void dump_to_stdout(const char* pFilename) +{ + TiXmlDocument doc(pFilename); + bool loadOkay = doc.LoadFile(); + if (loadOkay) + { + printf("\n%s:\n", pFilename); + dump_to_stdout( &doc ); // defined later in the tutorial + } + else + { + printf("Failed to load file \"%s\"\n", pFilename); + } +} + +// ---------------------------------------------------------------------- +// main() for printing files named on the command line +// ---------------------------------------------------------------------- +int main(int argc, char* argv[]) +{ + for (int i=1; i<argc; i++) + { + dump_to_stdout(argv[i]); + } + return 0; +} Property changes on: perception/zbar_barcode_reader_node/src/tinyxml.cpp ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Modified: perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-01-27 18:36:55 UTC (rev 984) +++ perception/zbar_barcode_reader_node/src/zbar_barcode_reader_node.cpp 2012-01-30 20:53:08 UTC (rev 985) @@ -9,6 +9,9 @@ #include <string.h> #include "std_msgs/String.h" #include <sstream> +#include "curl/curl.h" +#include "tinyxml.h" +#include <highgui.h> //Magick++ lib #include <Magick++.h> @@ -18,8 +21,63 @@ using namespace std; using namespace zbar; + static int writer(char *data, size_t size, size_t nmemb, + std::string *buffer) + { + // What we will return + int result = 0; + + // Is there anything in the buffer? + if (buffer != NULL) + { + // Append the data to the buffer + buffer->append(data, size * nmemb); + + // How much did we write? + result = size * nmemb; + } + + return result; + } + +static void* CURL_realloc(void *ptr, size_t size) +{ + /* There might be a realloc() out there that doesn't like reallocing + NULL pointers, so we take care of it here */ + if(ptr) + return realloc(ptr, size); + else + return malloc(size); +} + +struct memoryStruct { + char *memory; + size_t size; +}; + + size_t WriteMemoryCallback + (void *ptr, size_t size, size_t nmemb, void *data) + { + size_t realsize = size * nmemb; + struct memoryStruct *mem = (struct memoryStruct *)data; + + mem->memory = (char *) + CURL_realloc(mem->memory, mem->size + realsize + 1); + if (mem->memory) { + memcpy(&(mem->memory[mem->size]), ptr, realsize); + mem->size += realsize; + mem->memory[mem->size] = 0; + } + return realsize; + } + + class BarcodeReaderNode { + // struct memoryStruct { + // char *memory; + // size_t size; + // }; public: BarcodeReaderNode(ros::NodeHandle &n) : n_(n), it_(n_) @@ -35,7 +93,186 @@ { } + // void* CURL_realloc(void *ptr, size_t size) + // { + // /* There might be a realloc() out there that doesn't like reallocing + // NULL pointers, so we take care of it here */ + // if(ptr) + // return realloc(ptr, size); + // else + // return malloc(size); + // } + // size_t WriteMemoryCallback + // (void *ptr, size_t size, size_t nmemb, void *data) + // { + // size_t realsize = size * nmemb; + // struct memoryStruct *mem = (struct memoryStruct *)data; + + // mem->memory = (char *) + // CURL_realloc(mem->memory, mem->size + realsize + 1); + // if (mem->memory) { + // memcpy(&(mem->memory[mem->size]), ptr, realsize); + // mem->size += realsize; + // mem->memory[mem->size] = 0; + // } + // return realsize; + // } + // This is the writer call back function used by curl + // int writer(char *data, size_t size, size_t nmemb, + // std::string *buffer) + // { + // // What we will return + // int result = 0; + + // // Is there anything in the buffer? + // if (buffer != NULL) + // { + // // Append the data to the buffer + // buffer->append(data, size * nmemb); + + // // How much did we write? + // result = size * nmemb; + // } + + // return result; + // } + int getImage(std::string & image) + { + CURL *curl; // CURL objects + CURLcode res; + cv::Mat imgTmp; // image object + memoryStruct buffer; // memory buffer + + curl = curl_easy_init(); // init CURL library object/structure + + if(curl) + { + // set up the write to memory buffer + // (buffer starts off empty) + buffer.memory = NULL; + buffer.size = 0; + + // (N.B. check this URL still works in browser in case image has moved) + curl_easy_setopt(curl, CURLOPT_URL, image.c_str()); + curl_easy_setopt(curl, CURLOPT_VERBOSE, 1); // tell us what is happening + + // tell libcurl where to write the image (to a dynamic memory buffer) + + curl_easy_setopt(curl,CURLOPT_WRITEFUNCTION, WriteMemoryCallback); + curl_easy_setopt(curl,CURLOPT_WRITEDATA, (void *) &buffer); + + // get the image from the specified URL + res = curl_easy_perform(curl); + // decode memory buffer using OpenCV + imgTmp = cv::imdecode(cv::Mat(1, buffer.size, CV_8UC1, buffer.memory), CV_LOAD_IMAGE_UNCHANGED); + // display image (if we got / decoded it correctly) + + + if (!(imgTmp.empty())) + { + imshow("Image from URL", imgTmp); + } + cv::waitKey(3); + + // always cleanup + + curl_easy_cleanup(curl); + free(buffer.memory); + } + return 1; + } + + void findElement( TiXmlNode* pParent, std::string & picture) + { + if ( !pParent ) return; + + TiXmlNode* pChild; + //std::string picture ("Picture Not Found"); + std::string pstring = pParent->Value(); + // std::cerr << "pstring: " << pstring << std::endl; + size_t found=pstring.find("picture_high"); + if (found!=std::string::npos) + { + ROS_INFO_STREAM("First child: " << pParent->FirstChild()->Value()); + picture = pParent->FirstChild()->Value(); + return; + } + size_t found2=pstring.find("picture_low"); + if (found2!=std::string::npos && picture == "") + { + ROS_INFO_STREAM("First child: " << pParent->FirstChild()->Value()); + picture = pParent->FirstChild()->Value(); + return; + } + for ( pChild = pParent->FirstChild(); pChild != 0; pChild = pChild->NextSibling()) + { + findElement(pChild, picture); + // std::cerr << "in for " << std::endl; + } + } + + + int visBarcooPicture(std::string buffer) + { + // std::cerr << "buffer: " << buffer << std::endl; + TiXmlDocument doc; + doc.Parse((const char*)buffer.c_str(), 0, TIXML_ENCODING_UTF8); + doc.SaveFile ("text.xml"); + std::string picture; + findElement (&doc, picture); + ROS_INFO_STREAM ("Picture link: " << picture); + if (picture == "") + return -1; + getImage (picture); + return 1; + } + + + int callBarcoo(std::string bar_code, std::string & buffer) + { + char errorBuffer[CURL_ERROR_SIZE]; + // Our curl objects + CURL *curl; + CURLcode result; + // Create our curl handle + curl = curl_easy_init(); + std::string full_url = "http://www.barcoo.com/api/get_product_complete?pi=" + bar_code + "&pins=ean&format=xml&source=ias-tum"; + ROS_INFO_STREAM("full_url: " << full_url); + + if (curl) + { + // Now set up all of the curl options + curl_easy_setopt(curl, CURLOPT_ERRORBUFFER, errorBuffer); + curl_easy_setopt(curl, CURLOPT_URL, full_url.c_str()); + curl_easy_setopt(curl, CURLOPT_HEADER, 0); + curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1); + //curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, &BarcodeReaderNode::writer); + curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, writer); + curl_easy_setopt(curl, CURLOPT_WRITEDATA, &buffer); + + // Attempt to retrieve the remote page + result = curl_easy_perform(curl); + + // Always cleanup + curl_easy_cleanup(curl); + + // Did we succeed? + if (result == CURLE_OK) + { + ROS_INFO_STREAM ("CURLE_OK"); + return 1; + } + else + { + ROS_INFO_STREAM ( "CURLE_NOT OK"); + ROS_ERROR_STREAM ("Error: [" << result << "] - " << errorBuffer); + return -1; + } + } + return -1; + } + void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr) { @@ -43,7 +280,7 @@ ROS_INFO("[BarcodeReaderNode: ] Image received"); try { - cv_image = bridge_.imgMsgToCv(msg_ptr, "passthrough"); + cv_image = bridge_.imgMsgToCv(msg_ptr, "mono8"); } catch (sensor_msgs::CvBridgeException error) { @@ -71,6 +308,7 @@ int n = scanner.scan(image); // extract results + std::stringstream ss; for(Image::SymbolIterator symbol = image.symbol_begin(); symbol != image.symbol_end(); ++symbol) @@ -80,16 +318,30 @@ << " barcode value " << symbol->get_data()); std_msgs::String msg; - std::stringstream ss; - ss << symbol->get_type_name() << "," << symbol->get_data(); + + //ss << symbol->get_type_name() << "," << symbol->get_data(); + ss << symbol->get_data(); msg.data = ss.str(); barcode_pub_.publish(msg); } if (n == 0) - ROS_WARN("Barcode not found"); + { + ROS_WARN("Barcode not found"); + return; + } if (n < 0) - ROS_ERROR("Error occured while finding barcode"); + { + ROS_ERROR("Error occured while finding barcode"); + return; + } + + std::string buffer; + if (callBarcoo(ss.str(), buffer) == 1) + { + // std::cerr << "buffer after callBarcoo " << buffer << std::endl; + visBarcooPicture (buffer); + } // clean up image.set_data(NULL, 0); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2012-01-27 18:37:06
|
Revision: 984 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=984&view=rev Author: moesenle Date: 2012-01-27 18:36:55 +0000 (Fri, 27 Jan 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@gm...> Date: Fri Jan 27 19:36:01 2012 +0100 commit 260b7704746b8185e64e3ccb0284ec3054176ff2 Author: Dejan Pangercic <dej...@gm...> Date: Fri Jan 27 19:34:40 2012 +0100 zbar compiled w/o video support, cpp example of zbar client commit 60392bba300cd3b7908678100b1645ff182ef8d1 Author: Dejan Pangercic <dej...@cs...> Date: Sun Aug 29 22:04:18 2010 +0200 public export commit 24765aef5f57c082d078c5b599947ed71d0c8437 Author: Dejan Pangercic <dej...@cs...> Date: Sun Aug 29 22:02:29 2010 +0200 added a launch file commit c358c7210ee1c53d032b34118fb84b15c35b9c68 Author: Dejan Pangercic <dej...@cs...> Date: Sun Aug 29 21:59:21 2010 +0200 created a node for barcode reading, input: image output: type,value exchanged c for c++ interface tested TODO: find a n..... that will port to cop:-) commit 0fd52914acf001cc75df60a84dffc0395a2e769b Author: Dejan Pangercic <dej...@cs...> Date: Sun Aug 29 21:24:21 2010 +0200 c++ implementation of zbar commit 1c567129b5432e62765bd0e87f1bc18f2a126bd2 Author: Dejan Pangercic <dej...@cs...> Date: Sun Aug 29 21:23:38 2010 +0200 added dependency git-commit Modified Paths: -------------- perception/zbar_barcode_reader_node/CMakeLists.txt Added Paths: ----------- perception/zbar_barcode_reader_node/src/scan_image.cpp Modified: perception/zbar_barcode_reader_node/CMakeLists.txt =================================================================== --- perception/zbar_barcode_reader_node/CMakeLists.txt 2012-01-27 18:36:44 UTC (rev 983) +++ perception/zbar_barcode_reader_node/CMakeLists.txt 2012-01-27 18:36:55 UTC (rev 984) @@ -31,5 +31,8 @@ rosbuild_add_executable(zbar_barcode_reader src/zbar_barcode_reader.c) target_link_libraries(zbar_barcode_reader MagickWand MagickCore) +rosbuild_add_executable(zbar_barcode_reader_cpp src/scan_image.cpp) +target_link_libraries(zbar_barcode_reader_cpp MagickWand MagickCore Magick++) + rosbuild_add_executable(zbar_barcode_reader_node src/zbar_barcode_reader_node.cpp) target_link_libraries(zbar_barcode_reader_node Magick++) \ No newline at end of file Added: perception/zbar_barcode_reader_node/src/scan_image.cpp =================================================================== --- perception/zbar_barcode_reader_node/src/scan_image.cpp (rev 0) +++ perception/zbar_barcode_reader_node/src/scan_image.cpp 2012-01-27 18:36:55 UTC (rev 984) @@ -0,0 +1,54 @@ +#include <iostream> +#include <Magick++.h> +#include <zbar.h> +#define STR(s) #s + +using namespace std; +using namespace zbar; + +int main (int argc, char **argv) +{ + if(argc < 2) return(1); + +#ifdef MAGICK_HOME + // http://www.imagemagick.org/Magick++/ + // under Windows it is necessary to initialize the ImageMagick + // library prior to using the Magick++ library + Magick::InitializeMagick(MAGICK_HOME); +#endif + + // create a reader + ImageScanner scanner; + + // configure the reader + scanner.set_config(ZBAR_NONE, ZBAR_CFG_ENABLE, 1); + + // obtain image data + Magick::Image magick(argv[1]); // read an image file + int width = magick.columns(); // extract dimensions + int height = magick.rows(); + Magick::Blob blob; // extract the raw data + magick.modifyImage(); + magick.write(&blob, "GRAY", 8); + const void *raw = blob.data(); + + // wrap image data + Image image(width, height, "Y800", raw, width * height); + + // scan the image for barcodes + int n = scanner.scan(image); + + // extract results + for(Image::SymbolIterator symbol = image.symbol_begin(); + symbol != image.symbol_end(); + ++symbol) { + // do something useful with results + cout << "decoded " << symbol->get_type_name() + << " symbol \"" << symbol->get_data() << '"' << endl; + } + + // clean up + image.set_data(NULL, 0); + + return(0); +} Property changes on: perception/zbar_barcode_reader_node/src/scan_image.cpp ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |