diff --git a/CMakeLists.txt b/CMakeLists.txt
index 772a84fab7bccae47793e6404d4d05d74bc3274b..528813e83836e9853bce99a308a8f802352c2510 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,10 +6,11 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm tf2_ros sensor_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Eigen3 REQUIRED)
# ********************************************************************
# Add system and labrobotica dependencies here
@@ -60,7 +61,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_algorithm sensor_msgs
+ CATKIN_DEPENDS iri_base_algorithm tf2_ros sensor_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -76,6 +77,7 @@ catkin_package(
# ********************************************************************
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${EIGEN3_INCLUDE_DIR})
# include_directories(${<dependency>_INCLUDE_DIRS})
## Declare a cpp library
diff --git a/README.md b/README.md
index 6554f000b6afcfd83b7ed9cdb34a2dc208929d9e..ce839a52bba7f9d2e5b5054712443092cbcfabee 100644
--- a/README.md
+++ b/README.md
@@ -4,6 +4,8 @@ The iri_collision_manager project description
# ROS Interface
### Topic subscribers
+ - /**tf** (tf/tfMessage)
+ - /**tf** (tf/tfMessage)
- ~**imu** (sensor_msgs/Imu.msg)
### Parameters
diff --git a/cfg/CollisionManager.cfg b/cfg/CollisionManager.cfg
index 65e993f5f7711e2e6617fd4bfa2c02c5cb706844..507c498416663e779f03fcd26422eec648112124 100755
--- a/cfg/CollisionManager.cfg
+++ b/cfg/CollisionManager.cfg
@@ -41,6 +41,8 @@ gen = ParameterGenerator()
gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0)
gen.add("collision_acc_th", double_t, 0, "Threshold to detect a collision", 9.8, 0.1, 40.0)
gen.add("err_msg_rate", double_t, 0, "Rate to publish err messages", 0.5, 0.1, 1.0)
+gen.add("fixed_frame", str_t, 0, "Fixed frame", "map")
+gen.add("tf_timeout", double_t, 0, "Timeout to find a transform", 0.2, 0.1, 2.0)
exit(gen.generate(PACKAGE, "CollisionManagerAlgorithm", "CollisionManager"))
\ No newline at end of file
diff --git a/config/params.yaml b/config/params.yaml
index cf5c09c3fc359fce5a7a81c8d85829c383cf1f07..440b9a4469736b00a533533fef4f57aefec5591c 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -1,3 +1,5 @@
-rate: 10
-collision_acc_th: 9.8
+rate: 100
+tf_timeout: 0.2
+collision_acc_th: 5.0
err_msg_rate: 0.5
+fixed_frame: helena/base_link
diff --git a/include/collision_manager_alg_node.h b/include/collision_manager_alg_node.h
index 1990ed2880751e05abd00f8a3a6489d445d2763b..a08940da94906d64ceb465e67608914908268dd5 100644
--- a/include/collision_manager_alg_node.h
+++ b/include/collision_manager_alg_node.h
@@ -27,8 +27,11 @@
#include <iri_base_algorithm/iri_base_algorithm.h>
#include "collision_manager_alg.h"
+#include <Eigen/Eigen>
// [publisher subscriber headers]
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
#include <sensor_msgs/Imu.h>
// [service client headers]
@@ -42,9 +45,19 @@
class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>
{
private:
+ Eigen::Vector3d imu_local_acc_; ///< Local input linear acceleration.
+ bool new_imu_input_; ///< Flag to know that new input data has been received.
+ std::string imu_frame_; ///< IMU frame id.
+ Eigen::Transform<double,3,Eigen::Affine> tf_fixed_frame_imu_; ///< Transform from fixed frame to IMU frame.
+ bool tf_loaded_; ///< Flag to know that tf_fixed_frame_imu has been loaded before.
+
// [publisher attributes]
// [subscriber attributes]
+ tf2_ros::Buffer tf2_buffer;
+
+ tf2_ros::TransformListener tf2_listener;
+
ros::Subscriber imu_subscriber_;
void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
pthread_mutex_t imu_mutex_;
@@ -85,6 +98,15 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
~CollisionManagerAlgNode(void);
protected:
+ /**
+ * \brief Function to tranform linear local acceleration to fixed frame.
+ *
+ * This checks if imu frame id is different from fixed frame and if not already
+ * saved, it saves the transfrom.
+ *
+ * \param _global_acc The current global linear acceleration.
+ */
+ bool get_global_acc(Eigen::Vector3d& _global_acc);
/**
* \brief main node thread
*
diff --git a/launch/rosbag_and_rviz.launch b/launch/rosbag_and_rviz.launch
new file mode 100644
index 0000000000000000000000000000000000000000..87b62bd9f53e90839889cda0d65b2bf677e5c89d
--- /dev/null
+++ b/launch/rosbag_and_rviz.launch
@@ -0,0 +1,35 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+ <arg name="output" default="screen"/>
+ <arg name="rviz" default="true"/>
+ <arg name="map" default="false"/>
+ <arg name="rviz_cfg" default="$(find iri_collision_manager)/rviz/dogs.rviz"/>
+ <arg name="rosbag_dir" default="$(find iri_collision_manager)/rosbag/"/>
+ <arg name="rosbag_file" default="ref.bag"/>
+ <param name="/use_sim_time" value="true" />
+ <arg name="factor" default="1"/>
+ <arg name="sec" default="0"/>
+ <arg name="map_file" default="$(find iri_collision_manager)/rosbag/map/map.bag.yaml"/>
+
+ <node name="player_iri_landmarks_tracker"
+ pkg ="rosbag"
+ type="play"
+ output="$(arg output)"
+ args="--clock -r $(arg factor) -s $(arg sec) --pause $(arg rosbag_dir)$(arg rosbag_file)">
+ </node>
+
+ <node name="rviz_iri_collision_manager"
+ pkg ="rviz"
+ type="rviz"
+ if ="$(arg rviz)"
+ args="-d $(arg rviz_cfg)">
+ </node>
+
+ <node name="map_server"
+ pkg="map_server"
+ type="map_server"
+ if="$(arg map)"
+ args="$(arg map_file)">
+ </node>
+</launch>
diff --git a/launch/test.launch b/launch/test.launch
index 88af9cfb25273b2805f0f848e5ef27874a1ff914..cdc21d29f953ef144ea54f6411df0916443db1c7 100644
--- a/launch/test.launch
+++ b/launch/test.launch
@@ -5,12 +5,17 @@
<arg name="launch_prefix" default=""/>
<arg name="dr" default="false"/>
<arg name="collision_config_file" default="$(find iri_collision_manager)/config/params.yaml"/>
+ <arg name="imu_topic" default="/helena/sensors/bno055_imu/imu"/>
+
+ <arg name="sim_time" default="false"/>
+ <param name="/use_sim_time" value="$(arg sim_time)" />
<include file="$(find iri_collision_manager)/launch/node.launch">
<arg name="node_name" value="iri_collision_manager"/>
<arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="config_file" value="$(arg collision_config_file)"/>
+ <arg name="imu_topic" value="$(arg imu_topic)"/>
</include>
<node name="rqt_reconfigure_iri_collision_manager"
diff --git a/package.xml b/package.xml
index 8de7e139e2075b7088414ef4132eadf9a3de4c95..1342d3743761903e4469498994301c51fb85e666 100644
--- a/package.xml
+++ b/package.xml
@@ -52,6 +52,7 @@
<build_depend>iri_base_algorithm</build_depend>
<build_export_depend>iri_base_algorithm</build_export_depend>
<exec_depend>iri_base_algorithm</exec_depend>
+ <depend>tf2_ros</depend>
<depend>sensor_msgs</depend>
diff --git a/rviz/dogs.rviz b/rviz/dogs.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..e93389a4bdef6e7f6b81fc16a3dec81832f0627f
--- /dev/null
+++ b/rviz/dogs.rviz
@@ -0,0 +1,319 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ Splitter Ratio: 0.5058823823928833
+ Tree Height: 728
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: LaserScan
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ helena/base_footprint:
+ Value: false
+ helena/base_link:
+ Value: true
+ helena/camera_bottom_screw_frame:
+ Value: false
+ helena/camera_color_frame:
+ Value: false
+ helena/camera_color_optical_frame:
+ Value: false
+ helena/camera_depth_frame:
+ Value: false
+ helena/camera_depth_optical_frame:
+ Value: false
+ helena/camera_infra1_frame:
+ Value: false
+ helena/camera_infra1_optical_frame:
+ Value: false
+ helena/camera_infra2_frame:
+ Value: false
+ helena/camera_infra2_optical_frame:
+ Value: false
+ helena/camera_link:
+ Value: false
+ helena/front_left_axle:
+ Value: false
+ helena/front_left_hub:
+ Value: false
+ helena/front_left_wheel:
+ Value: false
+ helena/front_right_axle:
+ Value: false
+ helena/front_right_hub:
+ Value: false
+ helena/front_right_wheel:
+ Value: false
+ helena/front_sonar:
+ Value: false
+ helena/helena_box:
+ Value: false
+ helena/helena_realsense_support:
+ Value: false
+ helena/imu_bno055:
+ Value: true
+ helena/imu_bno055_base:
+ Value: false
+ helena/odom:
+ Value: true
+ helena/rear_left_axle:
+ Value: false
+ helena/rear_left_hub:
+ Value: false
+ helena/rear_left_wheel:
+ Value: false
+ helena/rear_right_axle:
+ Value: false
+ helena/rear_right_hub:
+ Value: false
+ helena/rear_right_wheel:
+ Value: false
+ helena/rear_sonar:
+ Value: false
+ helena/robosense:
+ Value: false
+ helena/robosense_base:
+ Value: false
+ helena/top_plate:
+ Value: false
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ helena/odom:
+ helena/base_footprint:
+ helena/base_link:
+ helena/front_left_axle:
+ helena/front_left_hub:
+ helena/front_left_wheel:
+ {}
+ helena/front_right_axle:
+ helena/front_right_hub:
+ helena/front_right_wheel:
+ {}
+ helena/front_sonar:
+ {}
+ helena/rear_left_axle:
+ helena/rear_left_hub:
+ helena/rear_left_wheel:
+ {}
+ helena/rear_right_axle:
+ helena/rear_right_hub:
+ helena/rear_right_wheel:
+ {}
+ helena/rear_sonar:
+ {}
+ helena/top_plate:
+ helena/helena_box:
+ {}
+ helena/helena_realsense_support:
+ helena/camera_bottom_screw_frame:
+ helena/camera_link:
+ helena/camera_color_frame:
+ helena/camera_color_optical_frame:
+ {}
+ helena/camera_depth_frame:
+ helena/camera_depth_optical_frame:
+ {}
+ helena/camera_infra1_frame:
+ helena/camera_infra1_optical_frame:
+ {}
+ helena/camera_infra2_frame:
+ helena/camera_infra2_optical_frame:
+ {}
+ helena/robosense_base:
+ helena/robosense:
+ {}
+ helena/imu_bno055_base:
+ helena/imu_bno055:
+ {}
+ Update Interval: 0
+ Value: true
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.10000000149011612
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic: /helena/odom
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Class: rviz_plugin_tutorials/Imu
+ Color: 204; 51; 204
+ Enabled: false
+ History Length: 1
+ Name: Imu
+ Topic: /helena/sensors/bno055_imu/imu
+ Unreliable: false
+ Value: false
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/LaserScan
+ Color: 138; 226; 52
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: LaserScan
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Points
+ Topic: /helena/sensors/scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: helena/odom
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 25.189504623413086
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 1.3301063776016235
+ Y: -0.2307669073343277
+ Z: -2.2700793743133545
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 1.5697963237762451
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 1.5953969955444336
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1025
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1853
+ X: 67
+ Y: 27
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index 33d0feb944e40a89ac2079a145593e995f93d35c..d578d06866751a2b9eb7246c78418fbb69003bc3 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -2,6 +2,7 @@
CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>()
+ ,tf2_listener(tf2_buffer)
{
//init class attributes if necessary
if(!this->private_node_handle_.getParam("rate", this->config_.rate))
@@ -11,6 +12,16 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
else
this->setRate(this->config_.rate);
+ if(!this->private_node_handle_.getParam("fixed_frame", this->config_.fixed_frame))
+ {
+ ROS_WARN("AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode: param 'fixed_frame' not found. Setting it to map.");
+ this->config_.fixed_frame = "map";
+ }
+
+ this->new_imu_input_ = false;
+ this->imu_frame_ = "";
+ this->tf_loaded_ = false;
+
// [init publishers]
// [init subscribers]
@@ -36,17 +47,103 @@ CollisionManagerAlgNode::~CollisionManagerAlgNode(void)
void CollisionManagerAlgNode::mainNodeThread(void)
{
//lock access to algorithm if necessary
- this->alg_.lock();
- ROS_DEBUG("CollisionManagerAlgNode::mainNodeThread");
+ // this->alg_.lock();
+ // ROS_DEBUG("CollisionManagerAlgNode::mainNodeThread");
+ this->imu_mutex_enter();
+
+ if (this->new_imu_input_)
+ {
+ Eigen::Vector3d global_acc;
+ if (get_global_acc(global_acc))
+ {
+ double acc_norm_2d = std::sqrt(std::pow(global_acc(0),2) + std::pow(global_acc(1),2));
+ double acc_angle_2d = std::atan2(global_acc(1), global_acc(0));
+ while (acc_angle_2d >= M_PI)
+ acc_angle_2d -= 2*M_PI;
+ while (acc_angle_2d < -M_PI)
+ acc_angle_2d += 2*M_PI;
+ if (acc_norm_2d > this->config_.collision_acc_th)
+ ROS_INFO_STREAM("CollisionManagerAlgNode::mainNodeThread -> collision detected: acc = " << acc_norm_2d << "; angle = " << acc_angle_2d);
+ }
+ this->new_imu_input_ = false;
+ }
+
// [fill msg structures]
+ /*
+ //tf2_listener example BEGIN
+ try{
+ std::string target_frame = "child_frame";
+ std::string source_frame = "parent_frame";
+ ros::Time time = ros::Time::now();
+ ros::Duration timeout = ros::Duration(0.1);
+ this->alg_.unlock();
+ bool tf2_exists = this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout);
+ this->alg_.lock();
+ if(tf2_exists){
+ geometry_msgs::PoseStamped stamped_pose_in;
+ stamped_pose_in.header.stamp = time;
+ stamped_pose_in.header.frame_id = source_frame;
+ stamped_pose_in.pose.position.x = 1.0;
+ stamped_pose_in.pose.position.y = 0.0;
+ stamped_pose_in.pose.position.z = 0.0;
+ tf2::Quaternion quat_tf;
+ quat_tf.setRPY(0.0, 0.0, 1.5709);
+ geometry_msgs::Quaternion quat_msg;
+ tf2::convert(quat_tf, quat_msg);
+ stamped_pose_in.pose.orientation = quat_msg;
+ double yaw, pitch, roll;
+ tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
+ ROS_INFO("Original pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", stamped_pose_in.header.frame_id.c_str(), stamped_pose_in.pose.position.x, stamped_pose_in.pose.position.y, stamped_pose_in.pose.position.z, yaw, pitch, roll);
+ geometry_msgs::PoseStamped stamped_pose_out;
+ geometry_msgs::TransformStamped transform;
+ transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);
+ quat_msg = transform.transform.rotation;
+ tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
+ ROS_INFO("Found transform betwen frames (%s-->%s) with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", source_frame.c_str(), target_frame.c_str(), transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z, yaw, pitch, roll);
+ tf2::doTransform(stamped_pose_in, stamped_pose_out, transform);
+ quat_msg = stamped_pose_out.pose.orientation;
+ tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);
+ ROS_INFO("Transformed pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]", stamped_pose_out.header.frame_id.c_str(), stamped_pose_out.pose.position.x, stamped_pose_out.pose.position.y, stamped_pose_out.pose.position.z, yaw, pitch, roll);
+ ROS_INFO("---");
+ }else{
+ ROS_WARN("No transform found from '%s' to '%s'", source_frame.c_str(), target_frame.c_str()); }
+ }catch (tf2::TransformException &ex){
+ ROS_ERROR("TF2 Exception: %s",ex.what()); }
+ //tf2_listener example END
+ */
+
+
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
// [publish messages]
-
- this->alg_.unlock();
+
+ this->imu_mutex_exit();
+ // this->alg_.unlock();
+}
+
+bool CollisionManagerAlgNode::get_global_acc(Eigen::Vector3d& _global_acc)
+{
+ if (this->config_.fixed_frame != this->imu_frame_ || !this->tf_loaded_)
+ {
+ geometry_msgs::TransformStamped tf_fixed_frame_imu_msg;
+ if (!this->tf2_buffer.canTransform(this->config_.fixed_frame, this->imu_frame_, ros::Time::now(), ros::Duration(this->config_.tf_timeout)))
+ {
+ ROS_WARN_STREAM("CollisionManagerAlgNode::get_global_acc -> Can't transform from " << this->config_.fixed_frame << " to " << this->imu_frame_ << " at " << ros::Time::now());
+ this->tf_loaded_ = false;
+ return false;
+ }
+ tf_fixed_frame_imu_msg = this->tf2_buffer.lookupTransform(this->config_.fixed_frame, this->imu_frame_, ros::Time::now());
+ Eigen::Quaternion<double> rot(
+ tf_fixed_frame_imu_msg.transform.rotation.w, tf_fixed_frame_imu_msg.transform.rotation.x, tf_fixed_frame_imu_msg.transform.rotation.y, tf_fixed_frame_imu_msg.transform.rotation.z);
+
+ this->tf_fixed_frame_imu_ = Eigen::Transform<double,3,Eigen::Affine>(rot);
+ this->tf_loaded_ = true;
+ }
+ _global_acc = this->tf_fixed_frame_imu_*this->imu_local_acc_;
+ return true;
}
/* [subscriber callbacks] */
@@ -57,14 +154,9 @@ void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this->imu_mutex_enter();
- double acc_norm_2d = std::sqrt(std::pow(msg->linear_acceleration.x,2) + std::pow(msg->linear_acceleration.y,2));
- double acc_angle_2d = std::atan2(msg->linear_acceleration.y, msg->linear_acceleration.x);
- while (acc_angle_2d >= M_PI)
- acc_angle_2d -= 2*M_PI;
- while (acc_angle_2d < -M_PI)
- acc_angle_2d += 2*M_PI;
- if (acc_norm_2d > this->config_.collision_acc_th)
- ROS_INFO_STREAM("CollisionManagerAlgNode::imu_callback -> collision detected: acc = " << acc_norm_2d << "; angle = " << acc_angle_2d);
+ this->new_imu_input_ = true;
+ this->imu_frame_ = msg->header.frame_id;
+ this->imu_local_acc_ = Eigen::Vector3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();