git commit -m "first commit for v2"

Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(agc_radar)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++17)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  message_generation
+)
+
+ add_message_files(
+   FILES
+   agc_radar_msg.msg
+ )
+
+ add_service_files(
+   FILES
+   agc_radar_config.srv
+)
+
+# Generate added messages and services with any dependencies listed here
+ generate_messages(
+   DEPENDENCIES
+   std_msgs
+ )
+
+#catkin_package(
+#  #INCLUDE_DIRS include
+#  #LIBRARIES agc_radar
+#  CATKIN_DEPENDS message_generation roscpp std_msgs message_runtime gpio_handling
+#  #DEPENDS system_lib
+#)
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  # include
+  ${catkin_INCLUDE_DIRS}
+  # $HOME/catkin_ws/devel/include
+)
+
+add_executable(agc_radar src/agc_radar.cpp)
+add_dependencies(agc_radar ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(agc_radar ${catkin_LIBRARIES})

Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/msg/agc_radar_msg.msg

@@ -0,0 +1,4 @@
+Header header
+float64 Schutzzeit
+float64 Stopzeit
+bool obsctacle_stop

Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>agc_radar</name>
+  <version>0.0.0</version>
+  <description>The agc_radar package</description>
+
+  <maintainer email="tsprifl@todo.todo">tsprifl</maintainer>
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>gpio_handling</build_depend>
+  <!-- <build_depend>custom_messages</build_depend> -->
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>message_generation</build_export_depend>
+  <build_export_depend>gpio_handling</build_export_depend>
+  <!--<build_export_depend>custom_messages</build_export_depend> -->
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>gpio_handling</exec_depend>
+  <!--<exec_depend>custom_messages</exec_depend> -->
+
+  <export>
+  </export>
+
+</package>

Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar.cpp

@@ -0,0 +1,134 @@
+#include <ros/ros.h>
+#include <std_msgs/Bool.h>
+#include "agc_radar/agc_radar_msg.h" // for publishing
+#include "agc_radar/agc_radar_config.h" // for service
+#include "gpio_handling/gpio_get_config.h"
+#include "gpio_handling/gpio_set_config.h"
+#include "gpio_handling/gpio_get_pin.h"
+#include "gpio_handling/gpio_set_pin.h"
+#include <typeinfo>
+
+/*************************************************************************************************
+*** Service Client (GPIO-handling)
+**************************************************************************************************/
+ros::ServiceClient gpio_get_config_client;
+ros::ServiceClient gpio_set_config_client;
+ros::ServiceClient gpio_get_pin_client;
+ros::ServiceClient gpio_set_pin_client;
+
+/*************************************************************************************************
+*** Services
+**************************************************************************************************/
+gpio_handling::gpio_get_config gpio_get_config_srv;
+gpio_handling::gpio_set_config gpio_set_config_srv;
+gpio_handling::gpio_get_pin gpio_get_pin_srv;
+gpio_handling::gpio_set_pin gpio_set_pin_srv;
+
+/*************************************************************************************************
+*** Messages
+**************************************************************************************************/
+agc_radar::agc_radar_msg agc_radar_msg;
+
+bool config_Service_callback(agc_radar::agc_radar_config::Request &req,
+                             agc_radar::agc_radar_config::Request &res){
+    agc_radar_msg.Schutzzeit = req.Schutzzeit;
+    ROS_INFO("set Schutzzeit to: %f", req.Schutzzeit);
+    return true;
+}
+
+int main(int argc, char **argv)
+{
+    //defaults
+    agc_radar_msg.Schutzzeit = 1;
+    agc_radar_msg.Stopzeit = 0.01;
+
+    ros::init(argc, argv, "agc_radar");
+    ros::NodeHandle nh;
+    ROS_INFO("Node %s started.", ros::this_node::getName().c_str());
+
+    ros::Publisher agc_radar_pub = nh.advertise<agc_radar::agc_radar_msg>("agc_radar", 10);
+    ROS_INFO("Publishing on topic %s", agc_radar_pub.getTopic().c_str());
+    ros::Rate loop_rate(100);
+
+    ros::ServiceServer config_server = nh.advertiseService("agc_radar_config", config_Service_callback);
+    ROS_INFO("Service Server for radar configuration started. Call it with \"rosservice call %s\"", config_server.getService().c_str());
+
+    /* Configure Service Clients */
+    gpio_get_config_client = nh.serviceClient<gpio_handling::gpio_get_config>("gpio_get_config");
+    gpio_set_config_client = nh.serviceClient<gpio_handling::gpio_set_config>("gpio_set_config");
+    gpio_get_pin_client = nh.serviceClient<gpio_handling::gpio_get_pin>("gpio_get_pin");
+    gpio_set_pin_client = nh.serviceClient<gpio_handling::gpio_set_pin>("gpio_set_pin");
+
+    uint64_t begin = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
+    uint64_t begin_stop = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
+    gpio_get_pin_srv.request.pinNumber = {4};
+
+    while(ros::ok())
+    {
+        gpio_get_pin_client.call(gpio_get_pin_srv);
+        
+        if(gpio_get_pin_srv.response.pinValue[0] == true) // radar detected an Object
+        {    
+            if ((uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec >= begin + agc_radar_msg.Stopzeit * 1e9)
+            {
+                agc_radar_msg.obsctacle_stop = true;
+            }
+            begin_stop = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
+        }
+        else
+        {
+            if ((uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec >= begin_stop + agc_radar_msg.Schutzzeit * 1e9)
+            {
+                agc_radar_msg.obsctacle_stop = false;
+            }
+            begin = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
+        }
+
+        /*
+        if(gpio_get_msg.pin_value[4] == true) // radar detected an Object
+        {
+            sum = (0.99*sum + 0.01);
+        }
+        else
+        {
+            sum = (0.99*sum - 0.01);
+        }
+
+        if (sum < 0)
+        {
+            sum = 0;
+        }
+        if( sum > 1)
+        {
+            sum = 1;
+        }
+
+        ROS_INFO("sum: %lf", sum);
+
+        if (sum > 0.5)
+        {
+            agc_radar_msg.obsctacle_stop = true;
+            begin = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
+        }
+        else
+        {
+            if(  ((uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec) >= (begin + agc_radar_msg.Schutzzeit)  )
+            {
+                agc_radar_msg.obsctacle_stop = false;
+            }
+        }
+*/
+
+        
+
+        agc_radar_msg.header.stamp = ros::Time::now();
+        agc_radar_pub.publish(agc_radar_msg);
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+
+    return 0;
+}
+

Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar_config.cpp

@@ -0,0 +1,28 @@
+#include "ros/ros.h"
+#include "agc_radar/agc_radar_config.h"
+
+int main(int argc, char **argv)	// Node Main Function
+{
+ros::init(argc, argv, "agc_radar_config"); // Initializes Node Name		
+if (argc != 2)				
+{
+    ROS_INFO("cmd : rosrun own own_client arg0 ");
+    ROS_INFO("arg0: Schutzzeit, type: float64\n   arg1: Stopzeit, type: float64");
+return 1;
+}
+
+char buffer [128];
+sprintf(buffer, "%s/agc_radar_config", ros::this_node::getNamespace().c_str());
+ros::NodeHandle nh; 	
+ros::ServiceClient config_client = nh.serviceClient<agc_radar::agc_radar_config>(buffer); 
+
+// Declares the 'srv' service that uses the 'SrvTutorial' service file
+agc_radar::agc_radar_config srv;
+
+// Parameters entered when the node is executed as a service request value are stored at 'a' and 'b'
+srv.request.Schutzzeit = atoll(argv[1]);
+srv.request.Stopzeit = atoll(argv[2]);
+config_client.call(srv);
+
+    return 0;
+}   

Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/srv/agc_radar_config.srv

@@ -0,0 +1,3 @@
+float64 Schutzzeit
+float64 Stopzeit
+---