Skip to content
Snippets Groups Projects
Commit 7f486d9a authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added tf from fixed frame to imu

parent 59515da5
No related branches found
No related tags found
No related merge requests found
...@@ -6,10 +6,11 @@ find_package(catkin REQUIRED) ...@@ -6,10 +6,11 @@ find_package(catkin REQUIRED)
# ******************************************************************** # ********************************************************************
# Add catkin additional components here # 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 ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3 REQUIRED)
# ******************************************************************** # ********************************************************************
# Add system and labrobotica dependencies here # Add system and labrobotica dependencies here
...@@ -60,7 +61,7 @@ catkin_package( ...@@ -60,7 +61,7 @@ catkin_package(
# ******************************************************************** # ********************************************************************
# Add ROS and IRI ROS run time dependencies # 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 # Add system and labrobotica run time dependencies here
# ******************************************************************** # ********************************************************************
...@@ -76,6 +77,7 @@ catkin_package( ...@@ -76,6 +77,7 @@ catkin_package(
# ******************************************************************** # ********************************************************************
include_directories(include) include_directories(include)
include_directories(${catkin_INCLUDE_DIRS}) include_directories(${catkin_INCLUDE_DIRS})
include_directories(${EIGEN3_INCLUDE_DIR})
# include_directories(${<dependency>_INCLUDE_DIRS}) # include_directories(${<dependency>_INCLUDE_DIRS})
## Declare a cpp library ## Declare a cpp library
......
...@@ -4,6 +4,8 @@ The iri_collision_manager project description ...@@ -4,6 +4,8 @@ The iri_collision_manager project description
# ROS Interface # ROS Interface
### Topic subscribers ### Topic subscribers
- /**tf** (tf/tfMessage)
- /**tf** (tf/tfMessage)
- ~**imu** (sensor_msgs/Imu.msg) - ~**imu** (sensor_msgs/Imu.msg)
### Parameters ### Parameters
......
...@@ -41,6 +41,8 @@ gen = ParameterGenerator() ...@@ -41,6 +41,8 @@ gen = ParameterGenerator()
gen.add("rate", double_t, 0, "Main loop rate (Hz)", 10.0, 0.1, 1000.0) 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("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("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")) exit(gen.generate(PACKAGE, "CollisionManagerAlgorithm", "CollisionManager"))
\ No newline at end of file
rate: 10 rate: 100
collision_acc_th: 9.8 tf_timeout: 0.2
collision_acc_th: 5.0
err_msg_rate: 0.5 err_msg_rate: 0.5
fixed_frame: helena/base_link
...@@ -27,8 +27,11 @@ ...@@ -27,8 +27,11 @@
#include <iri_base_algorithm/iri_base_algorithm.h> #include <iri_base_algorithm/iri_base_algorithm.h>
#include "collision_manager_alg.h" #include "collision_manager_alg.h"
#include <Eigen/Eigen>
// [publisher subscriber headers] // [publisher subscriber headers]
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <sensor_msgs/Imu.h> #include <sensor_msgs/Imu.h>
// [service client headers] // [service client headers]
...@@ -42,9 +45,19 @@ ...@@ -42,9 +45,19 @@
class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm> class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>
{ {
private: 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] // [publisher attributes]
// [subscriber attributes] // [subscriber attributes]
tf2_ros::Buffer tf2_buffer;
tf2_ros::TransformListener tf2_listener;
ros::Subscriber imu_subscriber_; ros::Subscriber imu_subscriber_;
void imu_callback(const sensor_msgs::Imu::ConstPtr& msg); void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
pthread_mutex_t imu_mutex_; pthread_mutex_t imu_mutex_;
...@@ -85,6 +98,15 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio ...@@ -85,6 +98,15 @@ class CollisionManagerAlgNode : public algorithm_base::IriBaseAlgorithm<Collisio
~CollisionManagerAlgNode(void); ~CollisionManagerAlgNode(void);
protected: 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 * \brief main node thread
* *
......
<?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>
...@@ -5,12 +5,17 @@ ...@@ -5,12 +5,17 @@
<arg name="launch_prefix" default=""/> <arg name="launch_prefix" default=""/>
<arg name="dr" default="false"/> <arg name="dr" default="false"/>
<arg name="collision_config_file" default="$(find iri_collision_manager)/config/params.yaml"/> <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"> <include file="$(find iri_collision_manager)/launch/node.launch">
<arg name="node_name" value="iri_collision_manager"/> <arg name="node_name" value="iri_collision_manager"/>
<arg name="output" value="$(arg output)"/> <arg name="output" value="$(arg output)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/> <arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="config_file" value="$(arg collision_config_file)"/> <arg name="config_file" value="$(arg collision_config_file)"/>
<arg name="imu_topic" value="$(arg imu_topic)"/>
</include> </include>
<node name="rqt_reconfigure_iri_collision_manager" <node name="rqt_reconfigure_iri_collision_manager"
......
...@@ -52,6 +52,7 @@ ...@@ -52,6 +52,7 @@
<build_depend>iri_base_algorithm</build_depend> <build_depend>iri_base_algorithm</build_depend>
<build_export_depend>iri_base_algorithm</build_export_depend> <build_export_depend>iri_base_algorithm</build_export_depend>
<exec_depend>iri_base_algorithm</exec_depend> <exec_depend>iri_base_algorithm</exec_depend>
<depend>tf2_ros</depend>
<depend>sensor_msgs</depend> <depend>sensor_msgs</depend>
......
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
...@@ -2,6 +2,7 @@ ...@@ -2,6 +2,7 @@
CollisionManagerAlgNode::CollisionManagerAlgNode(void) : CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>() algorithm_base::IriBaseAlgorithm<CollisionManagerAlgorithm>()
,tf2_listener(tf2_buffer)
{ {
//init class attributes if necessary //init class attributes if necessary
if(!this->private_node_handle_.getParam("rate", this->config_.rate)) if(!this->private_node_handle_.getParam("rate", this->config_.rate))
...@@ -11,6 +12,16 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) : ...@@ -11,6 +12,16 @@ CollisionManagerAlgNode::CollisionManagerAlgNode(void) :
else else
this->setRate(this->config_.rate); 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 publishers]
// [init subscribers] // [init subscribers]
...@@ -36,17 +47,103 @@ CollisionManagerAlgNode::~CollisionManagerAlgNode(void) ...@@ -36,17 +47,103 @@ CollisionManagerAlgNode::~CollisionManagerAlgNode(void)
void CollisionManagerAlgNode::mainNodeThread(void) void CollisionManagerAlgNode::mainNodeThread(void)
{ {
//lock access to algorithm if necessary //lock access to algorithm if necessary
this->alg_.lock(); // this->alg_.lock();
ROS_DEBUG("CollisionManagerAlgNode::mainNodeThread"); // 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] // [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 srv structure and make request to the server]
// [fill action structure and make request to the action server] // [fill action structure and make request to the action server]
// [publish messages] // [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] */ /* [subscriber callbacks] */
...@@ -57,14 +154,9 @@ void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg ...@@ -57,14 +154,9 @@ void CollisionManagerAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg
//use appropiate mutex to shared variables if necessary //use appropiate mutex to shared variables if necessary
//this->alg_.lock(); //this->alg_.lock();
this->imu_mutex_enter(); this->imu_mutex_enter();
double acc_norm_2d = std::sqrt(std::pow(msg->linear_acceleration.x,2) + std::pow(msg->linear_acceleration.y,2)); this->new_imu_input_ = true;
double acc_angle_2d = std::atan2(msg->linear_acceleration.y, msg->linear_acceleration.x); this->imu_frame_ = msg->header.frame_id;
while (acc_angle_2d >= M_PI) this->imu_local_acc_ = Eigen::Vector3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z);
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);
//std::cout << msg->data << std::endl; //std::cout << msg->data << std::endl;
//unlock previously blocked shared variables //unlock previously blocked shared variables
//this->alg_.unlock(); //this->alg_.unlock();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment