Hello,
We are working on a heavy omnidirectional platform using orientable wheels. We are using teb_local_planner for navigation. While the overall performance of the planner is good, we are facing serious issues in certain situations, particularly when there are close-by objects.
In these situations, teb planner is constantly re-planning the path, resulting in two different plans alternating quickly.
One of the main characteristics of the orientable wheels is that the platform has a pretty slow reaction to orientation changes (it has to reconfigure the wheel's orientation), so it is not able to react to the path change fast enough. The result is that the robot gets stuck in the position, with the wheels "dancing" between the two required configurations for the two different alternating paths.
Here are a coupe of sample videos ([one](https://youtu.be/zUkB8gfuFtE), [two](https://youtu.be/HgpyNAnvDok)) (rviz of the real robot) in which it can be appreciated. The plan is alternating between two apparently safe paths, but the robot is unable to configure wheels in time to follow a plan before it changes. So it gets stuck and doesn't move.
Here is [another video](https://youtu.be/yIB_y_ZXc6M ) of the robot stuck in a similar situation. However, this [other video](https://youtu.be/Qpq7MRYuxd8) shows how the robot is able to travel one path in one direction, but gets stuck in the same way in the opposite. As is seen in the video, the robot is actually capable of do it, but the planner is not able to create an adequate plan. When manually setting an additional "waypoint", the robot is able to get out. It can be seen, however, how the robot struggles with alternating re-plans several times.
We have tried to penalize angular acceleration to compensate for the slow reaction of the robot. However, it doesn't seem to have any effect on the creation of alternating plans (which is actually the source of the problem) and higher acceleration limits seems to perform better (lots of not feasible trajectories with very slow angular accelerations).
Setting `legacy_obstacle_association` to true alleviates greatly the problem. It seems that including all the obstacles in the planning makes the robot to pass further away from obstacles and makes paths more stable. However, the problem persists when the robot needs to pass close to obstacles.
So far we haven't found or guess any way or parameter to force that the planner only changes the current plan under more strict conditions to avoid it alternate between two. Is there such an option? What other parameters can we tune to alleviate this behavior?
**EDIT:**
When re-planning, there doesn't seem to be any warning or feasibility check fail message, so both plans seems to be valid. The behavior looks like it is alternating between two very close local minima, and the noise from the sensors seems enough to make the planner jump from one minima to the other and back. Sensor noise makes the obstacles around the robot look a bit closer or farther away, which can be enough to trigger the jump.
Increasing the resolution of the map also alleviates the problem (higher map resolution mitigates the sensor noise effects), which also seems o point in this direction.
Maybe it could be interesting to introduce an option for not changing the current plan unless the feasibility check fails, even if another more optimal plan is computed.
Thank you and best regards.
Current planner configuration:
TebLocalPlannerROS:
odom_topic: odom
map_frame: map
# Trajectory
teb_autosize: true
dt_ref: 0.3
dt_hysteresis: 0.1
global_plan_overwrite_orientation: false
max_global_plan_lookahead_dist: 5.0
feasibility_check_no_poses: 30
allow_init_with_backwards_motion: true
# Robot
max_vel_x: 0.2
max_vel_x_backwards: 0.2
max_vel_y: 0.2
max_vel_theta: 0.5
acc_lim_x: 0.2
acc_lim_y: 0.2
acc_lim_theta: 0.5
min_turning_radius: 0.0
wheelbase: 0.0
cmd_angle_instead_rotvel: false
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "line"
line_start: [0.3, 0.0]
line_end: [-0.3, 0.0]
min_obstacle_dist: 0.45
inflation_dist: 0.75
# GoalTolerance
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.2
free_goal_vel: false
# Obstacles
include_costmap_obstacles: true
costmap_obstacles_behind_robot_dist: 2.0
obstacle_poses_affected: 30
legacy_obstacle_association: true
# Optimization
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: true
optimization_verbose: false
penalty_epsilon: 0.1
weight_max_vel_x: 2
weight_max_vel_y: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_y: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1 # it is an holonomic robot, must be low
weight_kinematics_forward_drive: 0
weight_kinematics_turning_radius: 0
weight_optimaltime: 1
weight_obstacle: 50
weight_dynamic_obstacle: 10 # not in use yet20.0
selection_alternative_time_cost: false
# Homotopy Class Planner
enable_homotopy_class_planning: false
enable_multithreading: true
selection_cost_hysteresis: 1.0
selection_obst_cost_scale: 100.0
selection_prefer_initial_plan: 0.95
selection_viapoint_cost_scale: 1.0
simple_exploration: false
max_number_classes: 4
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_keypoint_offset: 0.1
obstacle_heading_threshold: 0.45
visualize_hc_graph: false
# Global plan following
weight_viapoint: 10
global_plan_viapoint_sep: -1 # -1 to disable
↧
teb_local_planner: avoid constant path replanning
↧
Very strange turn behavior with TEB planner
I use TEB as the local planner for the robot's navigation.
The path of the original TEB planner is very reasonable, as Fig 1 shows.
fig 1
----------
But as it moves forward, the path becomes very strange. The path first knotted and then became complicated.
fig 2
----------
fig 3
----------
fig 4
----------
fig 5
----------
fig 6
----------
fig 7
----------
fig 8
----------
Clearly the initial path is the best, why will it change?
I'd like to know:
1. Causes of this problem.
2. Possible
ways to improve.
Can anyone give me some suggestion?
Thanks in advance ;-)
↧
↧
move_base receiving empty plan
I'm trying to write a global planner to use with move_base.
I'm printing the path in the console and it looks good, but I'm getting a warn from move_base telling me the plan is empty, even though I'm checking in the end of makePlan that it isn't.
This is the part of makePlan that adds the poses to the plan vector:
if (bestPath.size() > 0){
for (int i = bestPath.size()-1; i >=0; i--){
GridCell current = bestPath[i];
GridCell previous;
if (i != (bestPath.size()-1))
previous = bestPath[i + 1];
else
previous = current;
//Orient the bot towards target
tf::Vector3 vectorToTarget;
vectorToTarget.setValue(current.x - previous.x, current.y - previous.y, 0.0);
float angle = atan2((double)vectorToTarget.y(), (double)vectorToTarget.x());
geometry_msgs::PoseStamped pose = goal;
pose.pose.position.x = current.x;
pose.pose.position.y = current.y;
pose.pose.position.z = 0.0;
pose.pose.orientation = tf::createQuaternionMsgFromYaw(angle);
plan.push_back(pose);
}
ROS_DEBUG("Plan empty: %d", plan.empty());
return true;
}
And the warning I'm getting is: [WARN]: Received an empty transformed plan.
Any help will be greatly appreciated.
↧
Unable To goto Goal Position (Path Fail)
Dear All,
I am having a stupid problem.
I am trying to send my robot to a goal poisition (i have tried using Navfn, glabalplanner and carrot). However, it cannot seems to go there at all. The problem i think is (Please note its a multi robot system (two robots in gazebo simulation)):
selecting **global costmap** in rviz results in showing the robot at someheight from the ground (as shown in the picture).
This is my global costmap file:
global_costmap:
#Set the global and robot frames for the costmap
global_frame: map
robot_base_frame: chassis
#Set the update and publish frequency of the costmap
update_frequency: 5.0
publish_frequency: 2.0
#We'll use a map served by the map_server to initialize this costmap
static_map: true
rolling_window: false
footprint_padding: 0.02
This is costmap_common_params:
#This file contains common configuration options for the two costmaps used in the navigation stack for more details on the parameters in this file, and a full list of the parameters used by the costmaps, please see http://www.ros.org/wiki/costmap_2d
map_type: voxel
#Voxel grid specific parameters
origin_z: 0.0
z_resolution: 0.2
z_voxels: 10
unknown_threshold: 9
mark_threshold: 0
#Set the tolerance we're willing to have for tf transforms
transform_tolerance: 0.3
#Obstacle marking parameters
obstacle_range: 2.5
max_obstacle_height: 2.0
raytrace_range: 3.0
#The footprint of the robot and associated padding
footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]
footprint_padding: 0.01
#Cost function parameters
inflation_radius: 0.55
cost_scaling_factor: 10.0
#The cost at which a cell is considered an obstacle when a map is read from the map_server
lethal_cost_threshold: 100
#Configuration for the sensors that the costmap will use to update a map
#Configuration for the sensors that the costmap will use to update a map
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: hokuyo, data_type: LaserScan, topic: scan, marking: true, clearing: true, max_obstacle_height: 20.0, min_obstacle_height: 0.0}
base_local_planner_params is below:
#For full documentation of the parameters in this file, and a list of all the
#parameters available for TrajectoryPlannerROS, please see
#http://www.ros.org/wiki/base_local_planner
TrajectoryPlannerROS:
#Set the acceleration limits of the robot
acc_lim_th: 3.2
acc_lim_x: 2.5
acc_lim_y: 2.5
#Set the velocity limits of the robot
#max_vel_x: 0.65
max_vel_x: 1.5
min_vel_x: 0.1
#max_rotational_vel: 1.0
max_rotational_vel: 1.5
min_in_place_rotational_vel: 0.4
#The velocity the robot will command when trying to escape from a stuck situation
escape_vel: -0.1
#For this example, we'll use a holonomic robot
holonomic_robot: true
#Since we're using a holonomic robot, we'll set the set of y velocities it will sample
y_vels: [-0.3, -0.1, 0.1, -0.3]
#Set the tolerance on achieving a goal
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.05
#We'll configure how long and with what granularity we'll forward simulate trajectories
sim_time: 1.7
sim_granularity: 0.025
vx_samples: 3
vtheta_samples: 20
#Parameters for scoring trajectories
goal_distance_bias: 0.8
path_distance_bias: 0.6
occdist_scale: 0.01
heading_lookahead: 0.325
#We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example
dwa: true
#How far the robot must travel before oscillation flags are reset
oscillation_reset_dist: 0.05
#Eat up the plan as the robot moves along it
prune_plan: true
↧
rosjava: how to call Services of move_base
I want to call the costmap_clear service of a rosndk native move_base node from ROSjava.
I found an example in the Rosjava docu.
But i cant see the service when i start move_base.
Its not listed in rosnode info and not in rosservice list.
I tried starting the move_base node on my pc but still no services....
Do i have to somehow start the service before i can call it?
Thanks in advance
EDIT: only 2 services are listed under move_base:
/move_base/get_loggers
/move_base/set_logger_level
↧
↧
How to change the cost (time) to reach the goal using path planning algorithm ?
I had created the map using gmapping, and also used AMCL to localize the robot in its environment. I wanted to know how I can add other factor which can influence time to reach the goal, or on what factor the does it depend.
↧
Taking into account the unknown zones by the LocalPlanner
Dear all,
As far as I understood, the unknown zones are defined in the _GlobalCostMap_ in order to help the _GlobalPlanner_ to generate a _GlobalPlan_ which does not interfere the Unknown Zones. But, the _LocallPlanner_ does not take into account this information.
I was wondering if there is a way that the _LocalPlanner_ takes into account the information about the Unknown Zones?
The problem which I try to solve is that when the robot wants to avoid an obstacle, it enters inside an unknown zone which is not desired.
Thanks,
↧
Navigation issue
Hello,
I can't figure out why my real robot does not stop when it arrives close to its goal.
that is my configurations:
I have kinetic distro, ubuntu 16.04
real_sense d435 to make Laserscan topic, encoders for the odometry.
**main launch file:**
↧
cmd_vel is published but robot is not moving after giving the goal position.
Everything is working fine, but the robot is not moving. Even when I use teleportation to control it is also not working. I got this after running move base command
NODES
/
move_base (move_base/move_base)
ROS_MASTER_URI=http://localhost:11311
process[move_base-1]: started with pid [28224]
[ WARN] [1545303469.955427106, 1149.825000000]: Timed out waiting for transform from odom to map to become available before running costmap, tf error: canTransform: target_frame map does not exist. canTransform: source_frame odom does not exist.. canTransform returned after 1149.83 timeout was 0.1.
[ INFO] [1545303469.990131640, 1149.859000000]: Using plugin "static_layer"
[ INFO] [1545303469.998307483, 1149.867000000]: Requesting the map...
[ INFO] [1545303470.205332254, 1150.072000000]: Resizing costmap to 416 X 192 at 0.050000 m/pix
[ INFO] [1545303470.305643198, 1150.171000000]: Received a 416 X 192 map at 0.050000 m/pix
[ INFO] [1545303470.311369260, 1150.176000000]: Using plugin "obstacle_layer"
[ INFO] [1545303470.314155950, 1150.179000000]: Subscribed to Topics: scan
[ INFO] [1545303470.341124836, 1150.206000000]: Using plugin "inflation_layer"
[ INFO] [1545303470.411881941, 1150.275000000]: Using plugin "obstacle_layer"
[ INFO] [1545303470.414188596, 1150.278000000]: Subscribed to Topics: scan
[ INFO] [1545303470.439318102, 1150.300000000]: Using plugin "inflation_layer"
[ INFO] [1545303470.496783249, 1150.357000000]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1545303470.511101433, 1150.370000000]: Sim period is set to 0.05
[ WARN] [1545303470.524212047, 1150.383000000]: Trajectory Rollout planner initialized with param meter_scoring not set. Set it to true to make your settins robust against changes of costmap resolution.
[ INFO] [1545303471.509039101, 1151.356000000]: Recovery behavior will clear layer obstacles
[ INFO] [1545303471.514283272, 1151.361000000]: Recovery behavior will clear layer obstacles
[ INFO] [1545303471.560497836, 1151.407000000]: odom received!
↧
↧
MessageFilter [target=map LaserScan ]: Dropped 100.00% of messages so far
I am using Raspbeery Pi3B+ with ubuntu os to run ROS. Sick_tim571 LiDAR sensor the moving base is self-build moving robot with DC motors.
The problem is when i first run the launch file, the first error appears but it is still able to run and i am able to control the robot using teleop_twist_keyboard, but when i trying to use the 2D Nav Goal, the robot will not move and sometime lose its position in the map(showing an incorrect position).
When i run my launch file this error appears:
[ WARN] [1544687487.798549003]: MessageFilter [target=map LaserScan ]: Dropped 100.00% of messages so far. Please turn the [ros.costmap_2d.message_notifier] rosconsole logger to DEBUG for more information.
[ WARN] [1544687487.798549003]: MessageFilter [target=odom LaserScan ]: Dropped 100.00% of messages so far. Please turn the [ros.costmap_2d.message_notifier] rosconsole logger to DEBUG for more information.
My launch file is as the following:
↧
use move_base to drive car without map and obstacle avoidance
Hi I am building a MIT Racecar. At this moment, I just want to test drive the car with the "2D Nav Goal" from rviz. I do not need map, planner, or obstacle avoidance. I just want the car to drive to the goal no matter what.
How can I do this with move_base?
THanks
----------
Update from July 24, 2018
I am thinking use the move_base_mapless_demo.launch in husky_navigation and remove the obstacle layer from the map
↧
Unable to send UTM move base goals to Rover
Hello!
I am attempting to implement a GPS guided rover.
However when I send a UTM goal to /move_base_simple/goal
I get the following issue:
[ WARN] [1545940576.270461738]: MessageFilter [target=odom ]: Discarding message from [/move_base] due to empty frame_id. This message will only print once.
Followed by repeating:
[ WARN] [1545940576.270723864]: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empty
I checked if all my frames transformations were being published and they all seem to be correct, so I am at a loss in finding the source of the empty frame_id error.
Oddly when I send a move base command in the "map" frame_id it works fine.
rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "map"}, pose: {position: {x: 2.0, y: 1.0, z: 0.0}, orientation: {w: 1.0}}}'
Here are my navigation .yaml files:
costmap_local.yaml:
global_frame: odom
rolling_window: true
width: 10.0
height: 10.0
resolution: 0.05
plugins:
- {name: obstacles_laser, type: "costmap_2d::ObstacleLayer"}
- {name: inflation, type: "costmap_2d::InflationLayer"}
costmap_global_static.yaml:
global_frame: map
rolling_window: true
static_map: false
track_unknown_space: true
plugins:
- {name: static, type: "costmap_2d::StaticLayer"}
- {name: inflation, type: "costmap_2d::InflationLayer"}
costmap_common.yaml:
footprint: [[-0.25, -0.33], [-0.25, 0.33], [0.25, 0.33], [0.25, -0.33]]
footprint_padding: 0.01
robot_base_frame: base_footprint
update_frequency: 1
publish_frequency: 3.0
transform_tolerance: 10
resolution: 0.05
obstacle_range: 5.5
raytrace_range: 6.0
#layer definitions
static:
map_topic: /map
subscribe_to_updates: true
obstacles_laser:
observation_sources: laser
laser: {data_type: LaserScan, clearing: true, marking: true, topic: scan, inf_is_valid: true}
inflation:
inflation_radius: .5
Here is my tf tree: https://imgur.com/a/byK7ZIL
Let me know if there is anything else that can be provided!
↧
yocs_velocity_smoother is not publishing any velocity command
I want to use the yocs_velocity_smoother node to smooth the velocity command published from move_base node. I remapped topic cmd_vel to raw_cmd_vel for the move_base node, and remapped smooth_cmd_vel to cmd_vel for the yocs_velocity_smoother node. However, all velocity command on cmd_vel topic is zero.
***How can I get the velocity smoother working properly?***
My move_base node launch code:
↧
↧
how to change the Rotating speed of "rotate_recovery" in move_base
Hello everyone, I have a question ,the rotating speed of "rotate_recovery" in move_base is too fast for my own robot.I want to slow down the speed of "rotate_recovery". Does anyone know which part of the code should be changed or just modify some parameters to solve this problem?
↧
How to use the DWB local planner
Dear all,
I was wondering if you can help me to find out how to use the [DWB_local_planner](https://github.com/locusrobotics/robot_navigation/tree/master/dwb_local_planner) in [move_base](http://wiki.ros.org/move_base).
I have already installed all the packages in the [This](https://github.com/locusrobotics/robot_navigation).
You can find the part of the launcher where I call the *move_base*.
↧
Explore_lite exploration finished
Hi guys,
At the moment, I'm working with move_base and explore_lite package to achieve exploring the unknown spaces. At some point in time its done with exploration. After this, I want to continue with my own navigation codebase to look for and navigate to objects that it had seen during the exploration. However, where do I retrieve such an information when it's done with exploration from explore_lite package?
Or is it even possible to cancel explore_lite node when it's done and continue further with the navigation node?
↧
navsat_transform_node UTM->map transform is highly inaccurate
I am trying to achieve gps waypoint navigation. I am using the UTM->map transform provided by the navsat_transform_node to convert a Lat,Long into a goal for move_base. I am having wildly inaccurate results for where my goals are occasionally up to 10 meters (OR MORE) in any direction around the robot. Sometimes I will pick a point 10 meters in front of the robot and it will place the goal to the left of the robot.
The transform I listen to is utm->odom but even when I listen to utm->map and send m goal in the map frame I still get the same results? Is anyone able to recommend a specific tuning to fix this issue or perhaps an alternative package/approach?
Thank you in advance
↧
↧
How to modify move_base minimum velocities?
I am using navigation stack in order to navigate my robot. I know that after giving a goal, move_pase constructs global and local plans. Using local plan, move_base publisher to /cmd_vel topic in order to move my robot.
I have written yaml file to set the parameters for the local plan like this:
ecovery_behavior_enabled: true
clearing_rotation_allowed: true
TrajectoryPLannerROS:
acc_lim_x: 1
acc_lim_y: 0 # as we have a differential robot
acc_lim_theta: 1.6
max_vel_x: 1.6 # configure, was 0.2
min_vel_x: 0.8 # was 0.1
max_vel_y: 0.0 # zero for a differential drive robot
min_vel_y: 0.0
max_vel_theta: 1.0 # 0.35
min_vel_theta: 0.0 #-0.35
min_in_place_vel_theta: 0.4 #0.25
min_rot_vel: 0.1
escape_vel: 0.0
holonomic_robot: false
meter_scoring: true
xy_goal_tolerance: 0.3 # 0.15, 0.3->about 20 cm
yaw_goal_tolerance: 0.20 # 0.25, about 11 degrees
latch_xy_goal_tolerance: true #false
sim_time: 4 # <2 values not enough time for path planning and can't go through narrow
sim_granularity: 0.05 # How frquent should the points on trajectory be examined
angular_sim_granularity: 0.025
vx_samples: 5
vy_samples: 0 # 0 for differential robots
vtheta_samples: 5
controller_frequency: 15
pdist_scale: 0.8
gdist_scale: 0.4
occdist_scale: 0.01
oscillation_reset_dist: 0.2
heading_scoring: false
heading_lookahead: 0.1 # how far to look ahead in meters
heading_scoring_timestep: 0.8
dwa: true
publish_cost_grid_pc: false
global_frame_id: odom
simple_attractor: false
reset_distance: 4
prune_plan: true
As you can see, I specifically said that the minimum x vel is 0.8 because from my motor speed calculations:
def callback(self, msg):
transVelocity = msg.linear.x
rotVelocity = msg.angular.z
velDiff = (self.wheelSep * rotVelocity) / 2.0
leftPower = (transVelocity + velDiff) / self.wheelRadius
rightPower = (transVelocity - velDiff) / self.wheelRadius
self.pwm_left.ChangeDutyCycle(leftPower)
self.pwm_right.ChangeDutyCycle(rightPower)
When the x.linear velocity is small my wheels do not spin as it has not enough power. However, after setting a goal and listening to /cmd_vel topic I get this message:
linear:
x: 0.55
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: -0.368421052632
Why is linar X is below 0.8? having those values, from my callback formula it will only deliver 20% of duty cycle and it is not enough.
↧
How to set min and max angular values in DWAPlannerROS?
I want to set min and max values that DWAPlannerROS can send to cmd_vel as these values were empirically measured for my robot.
This is how I stated my yaml file:
recovery_behavior_enabled: true
clearing_rotation_allowed: true
DWAPlannerROS:
acc_lim_x: 1
acc_lim_y: 0
acc_lim_theta: 1.6
max_vel_trans: 1.6
min_vel_trans: 0.8
max_vel_x: 1.6
min_vel_x: 0.8
max_vel_y: 0.0
min_vel_y: 0.0
min_vel_theta: 10
max_vel_theta: 11
holonomic_robot: false
xy_goal_tolerance: 0.3
yaw_goal_tolerance: 0.20
latch_xy_goal_tolerance: true
sim_time: 4
sim_granularity: 0.05
vx_samples: 5
vy_samples: 0
vth_samples: 5
controller_frequency: 15
occdist_scale: 0.01
oscillation_reset_dist: 0.2
publish_cost_grid: false
global_frame_id: odom
simple_attractor: false
reset_distance: 4
prune_plan: true
However, I echoed the /cmd_vel topic and I can see that move_base still sends the values below 10 for angular.z:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 1.0
Why is it sending these values?
↧
layered costmap : how to subscribe to nav_msgs/path, mark as obstacles and update to master layer
i am new to ROS and coding, i am trying to write a costmap layer which subscribes to topic "nav_msgs/path"(from bag file which assumed as path of other robot) and mark those array of poses as obstacles and update to master layer so that robot avoids that path and replans the path. i have followed this tutorial http://wiki.ros.org/costmap_2d/Tutorials/Creating%20a%20New%20Layer as the example, here i facing the problem in updating the correct boundary values in update bound function and mark those values in master layer. the obstacles are marked at some distance before the robot and when the robot is moving, (its showing in RViz) obstacle path is also moving along with the robot even though i mention the boundary values using mark_x and mark_y in functionCB and updating boundary values accordingly by calling set bounds function in it. please dont mind i dont know how to post the questions in answers.ros.org, please kindly help me in solving problem. thanks and regards
1. List item
my .cpp file
#include
#include
PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::GridLayer, costmap_2d::Layer)
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;
namespace simple_layer_namespace
{
GridLayer::GridLayer() {}
void GridLayer::setBounds(double x, double y, double z)
{
*temp_min_x = std::min(*temp_min_x, x);
*temp_min_y = std::min(*temp_min_y, y);
*temp_max_x = std::max(*temp_max_x, x);
*temp_max_y = std::max(*temp_max_y, y);
updateBounds(x,y,z,temp_min_x,temp_min_y, temp_max_x, temp_max_y);
}
void GridLayer::functionCB(const nav_msgs::Path& msg)
{
geometry_msgs::PoseStamped pose;
std::vector vector=msg.poses;
for(auto it=vector.begin(); it!=vector.end(); ++it)
{
double mark_x, mark_y, yaw;
pose.pose=it->pose;
mark_x=pose.pose.position.x;
mark_y=pose.pose.position.y;
yaw = pose.pose.orientation.z;
my_bool = false;
setBounds(mark_x,mark_y, yaw);
unsigned int mx;
unsigned int my;
if(worldToMap(mark_x, mark_y, mx, my))
{
setCost(mx, my, LETHAL_OBSTACLE);
}
}
}
void GridLayer::onInitialize()
{
ros::NodeHandle nh("~/" + name_);
current_ = true;
my_bool = true;
default_value_ = NO_INFORMATION;
matchSize();
sub=nh.subscribe("/local_path", 100, &GridLayer::functionCB, this);
dsrv_ = new dynamic_reconfigure::Server(nh);
dynamic_reconfigure::Server::CallbackType cb = boost::bind(
&GridLayer::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
void GridLayer::matchSize()
{
Costmap2D* master = layered_costmap_->getCostmap();
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
master->getOriginX(), master->getOriginY());
}
void GridLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
{
enabled_ = config.enabled;
}
void GridLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (!enabled_)
return;
if(my_bool)
{
temp_min_x = min_x;
temp_max_y = min_y;
temp_max_x = max_x;
temp_max_y = max_y;
}
else
{
*min_x = std::min(*min_x, *temp_min_x);
*min_y = std::min(*min_y, *temp_min_y);
*max_x = std::max(*max_x, *temp_max_x);
*max_y = std::max(*max_y, *temp_max_y);
}
}
void GridLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
int max_j)
{
if (!enabled_)
return;
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = getIndex(i, j);
if (costmap_[index] == NO_INFORMATION)
continue;
master_grid.setCost(i, j, costmap_[index]);
}
}
}
} // end namespace
2 . List item
my .h file
#ifndef GRID_LAYER_H_
#define GRID_LAYER_H_
#include
#include
#include
#include
#include
#include
namespace simple_layer_namespace
{
class GridLayer : public costmap_2d::Layer, public costmap_2d::Costmap2D
{
public:
GridLayer();
virtual void onInitialize();
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,
double* max_y);
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
bool isDiscretized()
{
return true;
}
void functionCB(const nav_msgs::Path& msg);
virtual void matchSize();
void setBounds(double x, double y, double z);
private:
void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
dynamic_reconfigure::Server *dsrv_;
double mark_x, mark_y;
bool my_bool;
double *temp_min_x, *temp_min_y, *temp_max_x, *temp_max_y;
ros::Subscriber sub;
};
}
#endif
`
↧