Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
Loading...
/cmd_vel 。確保不會發生輪子打滑,並且機器人能夠直線遠端操作。
由於各種因素,定位可能會造成漂移,使用此功能去手動調整定位,是非常重要的。
請確保機器人以正確的直線速度移動。
Add-on costs required
產品功能列表
&
轉角速度校準檢查 - 使用 pub /cmd_vel 進行測試,以獲得準確的結果。



footprint: [[0.2, 0.2], [-0.2, 0.2], [-0.2, -0.2], [0.2, -0.2]]
#robot_radius: 0.18#footprint: [[0.2, 0.2], [-0.2, 0.2], [-0.2, -0.2], [0.2, -0.2]]
robot_radius: 0.18<node ...
<remap from="<original topic name>" to="/cmd_vel"/>
</node><node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.22 0 0.1397 0 0 0 base_link laser 100" />$ rosnode info /motors_ctrl
--------------------------------------------------------------------------------
Node [/motors_ctrl]
Publications:
* /odom [nav_msgs/Odometry]
* /odom_euler [std_msgs/String]
* /robot_batt_perc [std_msgs/Int16]
* /rosout [rosgraph_msgs/Log]
* /tf [tf2_msgs/TFMessage]
Subscriptions:
* /cmd_vel [geometry_msgs/Twist]


PebbleLocalPlanner:
d_min: 0.30
robot_frame: base_link
map_frame: map
xy_goal_tolerance: 0.15
yaw_goal_tolerance: 0.3925
kp_linear: 1.0
ki_linear: 0.0
kd_linear: 0.0
kp_angular: 1.0
ki_angular: 0.0
kd_angular: 0.1
max_vel_x: 0.3
max_vel_theta: 0.785
acc_lim_x: 0.5
acc_lim_theta: 0.785
allow_reverse: true
th_turn: 1.0472 #1.0472 : 60 deg, how far the robot faces from the waypoint before suppressing linear motion
local_obstacle_avoidance: true # whether to let the local planner do obsav. If false, rely on the global planner, and planning frequency must not be zero
N_lookahead: 3 # how many multiples of d_min to look ahead when eval'ing obstacles






























































from movel_seirios_msgs.srv import StringTrigger, StringTriggerResponse # like setbool#include <movel_seirios_msgs/StringTrigger.h><depend>movel_seirios_msgs</depend>find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
movel_seirios_msgs
)#!/usr/bin/env python
import rospy
from movel_seirios_msgs.srv import StringTrigger, StringTriggerResponse # like setbool
from std_msgs.msg import Bool, UInt8
class Pytest:
def __init__(self):
print('starting test node')
self.node = rospy.init_node('python_test')
self.service_start = rospy.Service('/external_process/trigger_detecting', StringTrigger, self.start_handler)
self.service_stop = rospy.Service('/external_process/stop_detecting', StringTrigger, self.stop_handler)
# IGNORE dummy_publisher - just to simulate UI cancel topic
self.dummy_publisher = rospy.Publisher('/cancel_publish', Bool, queue_size=1)
self.topic_process_cancel = rospy.Subscriber('/cancel_publish', Bool, self.handle_publish_cancel)
self.client_status = rospy.Publisher('/external_process/client_status', UInt8, queue_size=1)
self.flag = True
def start_handler(self, req):
print('inside start handler')
start_msg = StringTriggerResponse()
start_msg.success = True
start_msg.message = 'starting detect'
self.flag = True
self.publish_client_status()
while(self.flag):
print('executing something')
rospy.sleep(1)
return start_msg # return must be [success, message format]
def stop_handler(self, req):
print('inside stop handler')
self.flag = False
print('stop handler called')
stop_msg = StringTriggerResponse()
stop_msg.success = False
stop_msg.message = 'stopping detect'
return stop_msg
# IGNORE if you are not using a topic to stop your code
def handle_publish_cancel(self, req):
print("inside publish cancel", req.data)
cancel_trigger = req.data
if(cancel_trigger):
print('executing cancel stuff') # replace print statement
# IGNORE if you are not using launch files
def publish_client_status(self):
print("publish client status called")
cstatus = UInt8()
#Dummy code
#Decide how do you determine if your launch file is successful
#And replace the if-else statement
if (self.flag == True):
cstatus.data = 2 # if start_handler successfully called, task is successful and return 2
else:
cstatus.data = 3 # else task not successful, return 3
self.client_status.publish(cstatus)
if __name__ == '__main__':
py_test = Pytest()
#py_test.publish_cancel()
rospy.spin()#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/UInt8.h>
#include <movel_seirios_msgs/StringTrigger.h>
class Cpptest {
private:
ros::ServiceServer service_start;
ros::ServiceServer service_stop;
ros::Publisher dummy_publisher;
ros::Subscriber topic_process_cancel;
ros::Publisher client_status;
bool flag;
public:
Cpptest(ros::NodeHandle *nh) {
service_start = nh->advertiseService("/external_package/triggering_detect_cpp", &Cpptest::start_handler, this);
service_stop = nh->advertiseService("/external_package/stopping_detect_cpp", &Cpptest::stop_handler, this);
// Ignore dummy_publisher - just to simulate ui cancel button
dummy_publisher = nh->advertise<std_msgs::Bool>("/cancel_publish_cpp", 10);
topic_process_cancel = nh->subscribe("/cancel_publish_cpp", 1, &Cpptest::handle_publish_cancel, this);
client_status = nh->advertise<std_msgs::UInt8>("/external_process/client_status_cpp", 10);
}
bool start_handler(movel_seirios_msgs::StringTrigger::Request &req, movel_seirios_msgs::StringTrigger::Response &res) {
std::cout << "inside start handler\n";
res.success = true;
res.message = "starting detect";
//std::cout << res << "\n";
flag = true;
publish_client_status();
std::cout << "start handler flag "<< flag << "\n";
return true;
}
bool stop_handler(movel_seirios_msgs::StringTrigger::Request &req, movel_seirios_msgs::StringTrigger::Response &res) {
std::cout << "inside stop handler\n";
res.success = false;
res.message = "stopping detect";
//std::cout << res << "\n";
flag = false;
std::cout << "stop handler flag "<< flag << "\n";
return true;
}
// Ignore this method if not subscribing to stop topic
void handle_publish_cancel(const std_msgs::Bool& msg) {
std::cout << "inside handle publish cancel\n";
bool cancel_trigger;
cancel_trigger = msg.data;
//std::cout << "published msg " << cancel_trigger << "\n";
if(cancel_trigger){
std::cout << "do canceling stuff. cancel_trigger: " << cancel_trigger << "\n";
}
}
// Ignore this method if not using launch
void publish_client_status() {
std::cout << "publish client status called\n";
std_msgs::UInt8 cstatus;
if(flag) {
cstatus.data = 2;
}
else {
cstatus.data = 3;
}
std::cout << "client status " << cstatus << "2 for success, 3 for fail \n";
client_status.publish(cstatus);
}
};
int main(int argc, char** argv) {
ros::init(argc, argv, "cpp_test");
ros::NodeHandle nh;
Cpptest cpp_test(&nh);
ros::spin();
return 0;
} watchdog_rate: 2.0
watchdog_timeout: 0
service_req: true
# Below 4 params are COMPULSORY if service_req is true
service_start: "/external_process/trigger_detecting"
service_start_msg: "" # Optional, may put blank string
service_stop: "/external_process/stop_detecting"
service_stop_msg: "" # Optional, may put blank string
launch_req: false
# Below 3 params COMPULSORY if launch_req is true
launch_package:
launch_file:
launch_nodes:
topic_cancel_req: true
# topic_process_cancel required if topic_cancel_req is true
topic_process_cancel: "external_process/cancel_publish"
client_status: # Required if launch_req true, optional otherwise plugins:
- {name: detecting, type: 10, class: 'external_process_handler::ExternalProcessHandler'}detecting:
watchdog_rate: 2.0
watchdog_timeout: 0
service_req: true
launch_req: false
service_start: "/external_process/trigger_detecting" #required if service_req is true
service_start_msg: "" #optional if there is any particular msg needs to sent or else empty string
service_stop: "/external_process/stop_detecting" #required if service_req is true
service_start_msg: "" #optional if there is any particular msg needs to sent or else empty string
#launch_package: "yocs_velocity_smoother" #required if launch_req is true
#launch_file: "standalone.launch" #required if launch_req is true
#launch_nodes: "/nodelet_manager /velocity_smoother" #required if launch_req is true
topic_cancel_req: true
topic_process_cancel: "external_process/cancel_publish"


Timed Elastic Band (TEB)根據執行時間、與障礙物的距離、和運動學約束,來執行優化機器人的軌跡。
teb_local_planner 改善導航功能teb_local_planner 參數目標容錯Goal ToleranceTebLocalPlannerROS:
odom_topic: /odom
#odom_topic: /rtabmap/odom
map_frame: map
# Trajectory
teb_autosize: True
dt_ref: 0.3 #0.2
dt_hysteresis: 0.1
global_plan_overwrite_orientation: True
max_global_plan_lookahead_dist: 3.0
feasibility_check_no_poses: 5
# Robot
max_vel_x: 0.40 #0.25
max_vel_x_backwards: 0.4 #0.2
max_vel_theta: 1.0 #0.5
acc_lim_x: 0.5 # 0.2
acc_lim_theta: 0.6283 #0.5, 0.26
min_turning_radius: 0.0
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "polygon" #"circular"
radius: 0.38 # for type "circular"
#line_start: [-0.3, 0.0] # for type "line"
#line_end: [0.3, 0.0] # for type "line"
#front_offset: 0.2 # for type "two_circles"
#front_radius: 0.2 # for type "two_circles"
#rear_offset: 0.2 # for type "two_circles"
#rear_radius: 0.2 # for type "two_circles"
vertices: [ [0.26, 0.26], [0.26, -0.26], [-0.26
# GoalTolerance
xy_goal_tolerance: 0.2 #0.2
yaw_goal_tolerance: 0.1571
free_goal_vel: False
# Obstacles
min_obstacle_dist: 0.05
inflation_dist: 0.0
dynamic_obstacle_inflation_dist: 0.05
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 0.1
obstacle_poses_affected: 25 #30
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 5
# Optimization
no_inner_iterations: 5 #5
no_outer_iterations: 4 #4
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
weight_max_vel_x: 2 #2
weight_max_vel_theta: 1 #1
weight_acc_lim_x: 1 # 1
weight_acc_lim_theta: 1 # 1
weight_kinematics_nh: 1000 #1000
weight_kinematics_forward_drive: 1000 #1000
weight_kinematics_turning_radius: 1 #1 #only for car-like robots
weight_optimaltime: 1.0 #1
weight_obstacle: 50 #50
weight_viapoint: 5.0 #5.0 #1.0
weight_inflation: 0.1 #0.1
weight_dynamic_obstacle: 10 # not in use yet
selection_alternative_time_cost: False # not in use yet
# Homotopy Class Planner
enable_homotopy_class_planning: False #True
enable_multithreading: True
simple_exploration: False
max_number_classes: 2 #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
#ViaPoints
global_plan_viapoint_sep: 0.5 #negative if none
via_points_ordered: False #adhere to order of via points
#Feedback
publish_feedback: true #false
local_planner: "TebLocalPlannerROS"
parameter_name_linear: "max_vel_x"
parameter_name_angular: "max_vel_theta"min_particles: 50
max_particles: 1000 max_vel_x: 0.40 #0.25
max_vel_x_backwards: 0.4 #0.2
max_vel_theta: 1.0 #0.5
acc_lim_x: 0.5 # 0.2
acc_lim_theta: 0.6283 #0.5, 0.26 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "polygon" #"circular"
radius: 0.38 # for type "circular"
#line_start: [-0.3, 0.0] # for type "line"
#line_end: [0.3, 0.0] # for type "line"
#front_offset: 0.2 # for type "two_circles"
#front_radius: 0.2 # for type "two_circles"
#rear_offset: 0.2 # for type "two_circles"
#rear_radius: 0.2 # for type "two_circles"
vertices: [ [0.26, 0.26], [0.26, -0.26], [-0.26, -0.26], [-0.26,0.26] ] # for type "polygon" xy_goal_tolerance: 0.2 #0.2
yaw_goal_tolerance: 0.1571 min_obstacle_dist: 0.05 inflation_dist: 0.0
dynamic_obstacle_inflation_dist: 0.05
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 0.1
obstacle_poses_affected: 25 #30
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 5footprint: [ [0.26, 0.26], [0.26, -0.26], [-0.26, -0.26], [-0.26,0.26] ]
# robot_radius: 0.38 #0.38
# footprint_padding: 0.05
map_type: voxel
#track_unknown_space: true
obstacle_layer:
origin_z: -0.1
z_resolution: 1.8 #1.5 This must be higher than the z coordinate of the mounted lidar
z_voxels: 1
obstacle_range: 10.0 #10.0
raytrace_range: 15.0 #15.0
observation_sources: laser_scan_sensor
track_unknown_space: true
lethal_cost_threshold: 100
unknown_cost_value: 255
laser_scan_sensor: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, min_obstacle_height: 0.00, max_obstacle_height: 3.00}
#point_cloud_sensor: {sensor_frame: lslidar_c16_frame, data_type: PointCloud2, topic: /lslidar_c16/lslidar_point_cloud, marking: true, clearing: true}
lowbstacle_layer:
origin_z: -0.1
z_resolution: 1.8
z_voxels: 1
obstacle_range: 3.5 #if beyond this threshold, then will not mark as obstacle
raytrace_range: 5.0 #5.0 Lower this value to detect nearer obstacles with better accuracy
observation_sources: obs_cloud mock_scan #butt_scan1 butt_scan2
publish_voxel_map: true
track_unknown_space: true
lethal_cost_threshold: 100
unknown_cost_value: 255
obs_cloud:
data_type: PointCloud2
topic: /obstacles_cloud
marking: true
clearing: true
min_obstacle_height: 0.01
max_obstacle_height: 0.99
mock_scan:
data_type: LaserScan
topic: /obstacles_scan
marking: false
clearing: true
min_obstacle_height: 0.00
max_obstacle_height: 1.00
inf_is_valid: true
inflation_layer:
enabled: true
cost_scaling_factor: 6.0 #added in by John
inflation_radius: 0.39 #0.45 #Minimum value: 0.379
dynamic_obstacle_layer:
enabled: false
map_tolerance: 0.2
footprint_radius: 0.5
range: 2.0 plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: lowbstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: dynamic_obstacle_layer, type: "dynamic_obstacle_layer::DynamicLayer"} # Uncomment to apply dynamic_obstacle_layer
- {name: inflation_layer, type: "costmap_2d::InflationLayer"} plugins:
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: lowbstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
# - {name: range_sensor_layer, type: "range_sensor_layer::RangeSensorLayer"}
inflation_layer:
enabled: true
cost_scaling_factor: 6.0 #added in by John
inflation_radius: 0.39 #0.45 #Minimum value: 0.379obstacle_layer:
origin_z: -0.1
z_resolution: 1.8 #1.5 This must be higher than the z coordinate of the mounted lidar
z_voxels: 1
obstacle_range: 10.0 #10.0
raytrace_range: 15.0 #15.0
observation_sources: laser_scan_sensor
track_unknown_space: true
lethal_cost_threshold: 100
unknown_cost_value: 255
laser_scan_sensor: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, min_obstacle_height: 0.00, max_obstacle_height: 3.00}
#point_cloud_sensor: {sensor_frame: lslidar_c16_frame, data_type: PointCloud2, topic: /lslidar_c16/lslidar_point_cloud, marking: true, clearing: true}lowbstacle_layer:
origin_z: -0.1
z_resolution: 1.8
z_voxels: 1
obstacle_range: 3.5 #if beyond this threshold, then will not mark as obstacle
raytrace_range: 5.0 #5.0 Lower this value to detect nearer obstacles with better accuracy
observation_sources: obs_cloud mock_scan #butt_scan1 butt_scan2
publish_voxel_map: true
track_unknown_space: true
lethal_cost_threshold: 100
unknown_cost_value: 255
obs_cloud:
data_type: PointCloud2
topic: /obstacles_cloud
marking: true
clearing: true
min_obstacle_height: 0.01
max_obstacle_height: 0.99
mock_scan:
data_type: LaserScan
topic: /obstacles_scan
marking: false
clearing: true
min_obstacle_height: 0.00
max_obstacle_height: 1.00
inf_is_valid: true









