from movel_seirios_msgs.srv import StringTrigger, StringTriggerResponse # like setbool

#include <movel_seirios_msgs/StringTrigger.h>

#!/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

TebLocalPlannerROS:

  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

footprint: [ [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"}