Closest point to a fixed frame
Hello
Im trying to write a node that gives me the distance to a fixed frame of some object in my map. So for example a door. So I just want to analyse the position of the robot (based on laser scan) in the relation to this object. So I passed the TF tutorial. But not sure if the code is Ok. Any help?
So this is my code
#include <ros/ros.h>
#include <tf/transform_listener.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
listener.lookupTransform("/base_link", "/door1",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
double x = transform.getOrigin().x();
double y = transform.getOrigin().y();
double dist = sqrt(x*x + y*y);
ROS_INFO("%f", dist);
rate.sleep();
}
return 0;
};
And just labelled the door on my launch file like this
<node pkg="tf" type="static_transform_publisher" name="door1" args="1 0 0 0 0 0 /odom /door1 10"/>
So I think this part of the code give me the the distance to the door. Cause my laser is placed on the base_link. So it give me the distance between the door and the base link in every time step
while (node.ok()){
tf::StampedTransform transform;
try{
listener.lookupTransform("/base_link", "/door1",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
double x = transform.getOrigin().x();
double y = transform.getOrigin().y();
double dist = sqrt(x*x + y*y);
ROS_INFO("%f", dist);
rate.sleep();
Yes??? This is the visualisation code
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"
#include "visualization_msgs/Marker.h"
/**
* This tutorial demonstrates simple receipt of messages over the ROS system.
*/
class amcl_pose_listener {
public:
ros::NodeHandle n_;
//Message filters for subscribed sensor msgs
message_filters::Subscriber<geometry_msgs::PoseWithCovarianceStamped> pose_sub_; //Subscriber to a published pose msg (from a localiser such as AMCL, etc)
ros::Publisher traj_pub_, line_to_door_pub_; //Publish an trajectory
//The trajectory
visualization_msgs::Marker trajectory_points_, line_to_door_points_;
amcl_pose_listener(ros::NodeHandle n):
pose_sub_(n_, "amcl_pose", 100) //Subscriber to a pose msg "amcl_pose" (from AMCL)
{
//Set call back function for pose data
pose_sub_.registerCallback(boost::bind(&amcl_pose_listener::poseCallBack, this, _1));
//Publish a trajectory
traj_pub_ = n.advertise<visualization_msgs::Marker>("robot_fixed_pose_trajectory", 100);
line_to_door_pub_ = n.advertise<visualization_msgs::Marker>("wheelchair_to_door_trajectory", 100);
line_to_door_points_.points.resize(2);
}
//Pose data callback
void poseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& data_in)
{
//Init the trajectory history
trajectory_points_.header.frame_id = "map";
trajectory_points_.header.stamp = ros::Time::now();
trajectory_points_.ns = "robot_fixed_pose_trajectory";
trajectory_points_.type = visualization_msgs::Marker::LINE_STRIP;
trajectory_points_.action = visualization_msgs::Marker::ADD;
trajectory_points_.id = 0;
//Green colour
trajectory_points_.color.g = 1.0f;
trajectory_points_.color.a = 1.0;
trajectory_points_.scale.x = 0.1; //Sets the width of the LINE_STRIP
trajectory_points_.scale.y = 0.1; //Ignored if marker type is LINE_STRIP
//Init the trajectory history
line_to_door_points_.header.frame_id = "map";
line_to_door_points_.header.stamp = ros::Time::now();
line_to_door_points_.ns = "line_to_door";
line_to_door_points_.type = visualization_msgs::Marker::LINE_STRIP;
line_to_door_points_.action = visualization_msgs::Marker::MODIFY ...
You didn't use the "code"-button (the one with the 10101) or indent your code, so it was not displayed correctly and unreadable. I fixed that.
" code button"?? When I edit the code you mean? Any comment to my code??
Yes, during editing there is a button with something like 101010 on it that will format your posted code to be readable.
Ok. Thanks. But I think my code is ok, and doing what I want. I do not understand the comment of michikarg. Why my code is wrong?
I only changed the display of your code. I have not reviewed the contents.
AAh ok. Can you give me some advice or help about the content? Cause I think that my code is ok