0
$\begingroup$

Rosanswers logo

Hello

I would like to create my own node that can navigate my robot through some objects in the environment map using laser scan. So I just want to have some locations of objects in my environment map , so I need a list of objects and coordinates.Just need a ROS node that takes a list of objects with coordinates and posts a TF transform for every object...

How to fill my yaml map file with the object parameters. I have the yaml map file already created. Any help with the fill the map and TF transform please?

This is my object node.

#include "ros/ros.h" #include "std_msgs/String.h" #include "sensor_msgs/LaserScan.h" #include

sensor_msgs::LaserScan laser_scan; float min_range;

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg) {

std::vector<float> laser;
laser = msg->ranges;

int size_laser = laser.size();
for (int i=0;i<size_laser;i++){
    if (laser[i] < 0.01){
        laser[i] = 99999;
    }
    if (laser[i] > 45){
        laser[i] = 99999;
    }
}

min_range = 2;
int index_min;
for (int i=0;i<size_laser;i++){
    if (laser[i] < min_range){
        min_range = laser[i];
        index_min = i;
    ROS_INFO("Minimum Range = %f", min_range);
    }
}

for (int j=0;j<size_laser;j++){
    if (laser[j] > min_range + 0.5){
        laser[j] = 0;
    }
}


laser_scan = *msg;
laser_scan.ranges.clear();
laser_scan.ranges = laser;

}

int main(int argc, char **argv) {

ros::init(argc, argv, "object_node");

ros::NodeHandle n;
ROS_INFO("Minimum Range = %f", min_range);
ros::Subscriber sub = n.subscribe("scan", 1000, scanCallback);
ros::Publisher laser_pub = n.advertise<sensor_msgs::LaserScan>("closest_points", 100);

ros::Rate loop_rate(10);

while (ros::ok())
{


    laser_pub.publish(laser_scan);

    ros::spinOnce();

    loop_rate.sleep();
}

return 0;

}

And this is the yaml map file

image: pow_real_time.pgm resolution: 0.050000 origin: [-100.000000, -100.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196

and this is my launch file

Perform AMCL localisation: runs several nodes to generate odometry from laser scans (ICP) & IMU, loads a map of the POW assessment area, runs AMCL, plays back a dataset for localisation, and runs the visualiser with the correct visualisation parameters configured.

<node name="rosplay" pkg="rosbag" type="play" args="/home/Data/13-48-20.bag  --clock"/>

<node pkg="tf" type="static_transform_publisher" name="baselink_laser" args="0 0 0 0 0 0 /base_link /laser 10"/>
<node pkg="tf" type="static_transform_publisher" name="laser_imu" args="0 0 0 0 0 0 /laser /base_imu 10"/>
<node pkg="tf" type="static_transform_publisher" name="baselink_camera" args="0 0 0 0 0 0 /base_link /camera 10"/>

<!-- Start the map server node and specify the map file (*.pgm) and the map resolution in metres/pixel -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl_listener)/maps/pow_real_time.yaml"         output="screen"/>

<!--Start the Laser_scan_matcher package, to provide odometry from laser data (ICP)-->
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node" 
    name="laser_scan_matcher_node" output="screen">
    <param name="use_alpha_beta" value="true"/>
    <param name="max_iterations" value="10"/>
</node>

<!-- Start the gmapping node -->
<!--node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"/-->

<node pkg="amcl" type="amcl" name="amcl" respawn="true" output="screen">
    <param name="base_frame_id" value="base_link"/>
    <param name="odom_frame_id" value="world"/>
    <param name="global_frame_id" value="map"/>
    <param name="update_min_d" value="0.09"/>   
    <param name="update_min_a" value="0.09"/> 
    <param name="initial_pose_x" value="-1"/>
    <param name="initial_pose_y" value="0"/>
    <param name="initial_pose_a" value="-0.1"/>
    <param name="min_particles" value="6000"/> 
    <param name="max_particles" value="8000"/>
    <param name="odom_model_type" value="diff"/>
    <param name="kld_err" value="0.1"/> 
    <param name="resample_interval" value="1"/>
    <param name="odom_alpha1" value="0.2"/>   
    <param name="odom_alpha2" value="0.2"/>
    <param name="odom_alpha3" value="0.5"/>
    <param name="odom_alpha4" value="0.5"/>
    <param name="laser_max_beams" value="30"/>
</node>

<!--Start the "analyzer_packages"-->
<!-- acml_listener broadcasts markers for the position of the wheelchair, to visualize in Rviz-->
<node pkg="amcl_listener" type="amcl_pose_listener" name="amcl_listener" output="screen"/>
<!-- Object node records minimal ranges from laser, speed_node will record travel speed of wheelchair -->
<!--node pkg="pow_analyzer" type="object_node" name="object_node" output="screen"/> -->
<node pkg="pow_analyzer" type="max_range_node" name="max_range_node" output="screen"/>
<!--node pkg="pow_analyzer" type="speed_node" name="speed_node" output="screen"/> -->

<!-- Start an rviz node with a custom configuration for the viewpoint, map_server, trajectory, laser scans, etc -->
<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find pow_analyzer)/launch/pow_rviz.vcg"/>

Originally posted by Astronaut on ROS Answers with karma: 330 on 2012-08-09

Post score: 0

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

As far as i understand you you need only to read from each object its location. As you staid you have the location within the yaml map. So read it with a node and then create for each on a TF object which you publish. So, somethink like this for each object:

  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(object->x, object->y, object->z) );
  transform.setRotation( tf::Quaternion(object->rot->x, object->rot->y, object->rot->z) );
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", object->name));

In cases where your object coordinates are not within the world/map frame you may need to change also the "world" label.

I hope this helps a little.

P.S.: Check this tutorials

Originally posted by tlinder with karma: 663 on 2012-08-09

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by Astronaut on 2012-08-09:
No. I have my environment map. But still I need to label the objects. My object coordinates are not within the map frame. So I need to create a file with a list of object and their coordinates or something like that?. But how? Or I just create that list in the node?

Comment by Astronaut on 2012-08-09:
Do I label the object with their coordinate in the yaml map file or just within the node?

Comment by tlinder on 2012-08-09:
Well i am not really familiar with Yaml Maps. Can you giv me an example of one or two objects within that format? I assumed that you have the objects, there location and if the location is not already within the map frame the knowledge of how the object-frame is linked to the map-frame.

Comment by tlinder on 2012-08-09:
If you have this information (location, frame and frame relation) than you can use the code above.

Comment by Astronaut on 2012-08-09:
I just edit the object node file, the yaml map and launch file. Ok?

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.