add lgsvl_msgs.

src/common/modulecomm/shm/procsm.cpp

@@ -247,6 +247,9 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
 
  //       if(!mpASM->attach())
 
+
+
+
         bool bares = mpASM->create(sizeof(procsm_info)+nMaxPacCount*sizeof(procsm_head) + nBufSize);
         if(bares == false)   //Exist.
         {
@@ -267,7 +270,38 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
             }
             else
             {
-                std::cout<<" Exist,But Fail Attach."<<std::endl;
+                std::cout<<" Exist,But Fail Attach. Recreate "<<std::endl;
+                qint64 uptime = std::chrono::system_clock::now().time_since_epoch().count();
+                snprintf(strasmname,256,"%s_%lld",strsmname,uptime);
+                mpASMPtr->lock();
+                ASM_PTR * pasm = (ASM_PTR *)mpASMPtr->data();
+                pasm->mnshmsize = sizeof(procsm_info)+nMaxPacCount*sizeof(procsm_head) + nBufSize;
+                pasm->mnUpdateTime = uptime;
+                strncpy(pasm->mstrshmname,strasmname,256);
+                mpASMPtr->unlock();
+                bool bRecreate = mpASM->create(sizeof(procsm_info)+nMaxPacCount*sizeof(procsm_head) + nBufSize);
+                if(bRecreate)
+                {
+                    char * p = (char *)mpASM->data();
+                    if(p == NULL)
+                    {
+                        qDebug("Create SharedMemory Fail.");
+                        return;
+                    }
+                    mpASM->lock();
+                    mpinfo = (procsm_info *)p;
+                    mphead = (procsm_head *)(p+sizeof(procsm_info));
+                    mpinfo->mCap = nMaxPacCount;
+                    mpinfo->mnBufSize = nBufSize;
+                    mpinfo->mFirst = 0;
+                    mpinfo->mNext = 0;
+                    mpinfo->mLock = 0;
+                    mpASM->unlock();
+                }
+                else
+                {
+                    std::cout<<" Recreate Fail. Need Exit Program."<<std::endl;
+                }
             }
 
         }
@@ -621,18 +655,14 @@ int procsm::writemsg(const char *str, const unsigned int nSize)
     }
     mpASM->lock();
 
-
-
-//    unsigned int * pIndexFirst = (unsigned int *)mpASM->data();
-//    unsigned int * pIndexNext = pIndexFirst+1;
-    if(mpinfo->mLock == 1)
-    {
-        std::cout<<"ShareMemory have lock.Init."<<std::endl;
-        mpinfo->mLock = 0;
-        mpinfo->mFirst = 0;
-        mpinfo->mNext = 0;
-    }
-    mpinfo->mLock =1;
+//    if(mpinfo->mLock == 1)
+//    {
+//        std::cout<<"ShareMemory have lock.Init."<<std::endl;
+//        mpinfo->mLock = 0;
+//        mpinfo->mFirst = 0;
+//        mpinfo->mNext = 0;
+//    }
+//    mpinfo->mLock =1;
 WRITEMSG:
     char * pH,*pD;
     QDateTime dt;
@@ -645,6 +675,15 @@ WRITEMSG:
         unsigned int nRemove = mnMaxPacCount/3;
         if(nRemove == 0)nRemove = 1;
         MoveMem(nRemove);
+        if(mnMaxPacCount == 0)
+        {
+            std::cout<<" Warning: mnMaxPacCount == 0,need Recreate."<<std::endl;
+            mpASM->lock();
+            mnMaxPacCount = 1;
+            recreateasm(nSize);
+            return -1;
+        }
+//        std::cout<<" nPac : "<<nPac<<" Max: "<<mnMaxPacCount<<std::endl;
         goto WRITEMSG;
     }
     if(nPac == 0)

src/ros/catkin/src/lgsvl_msgs-master/CHANGELOG.rst

@@ -0,0 +1,39 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package lgsvl_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.0.4 (2020-10-21)
+------------------
+* Add VehicleOdometry.msg to README
+* Adding VehicleOdometry.
+* Update README and LICENSE
+* Update package description and maintainers
+* Add ultrasonic message.
+* Contributors: Guodong Rong, Hadi Tabatabaee, Piotr Jaroszek
+
+0.0.3 (2020-05-29)
+------------------
+* Update readme and license
+* Changing some Vector3's to Points where they make more sense.
+* Renaming CanBus to CanBusData.
+* Adding VehicleStateData.
+* Adding VehicleControlData.
+* Adding CanBus and DetectedRadarObject/Array.
+* Making package hybrid ROS1/ROS2 package.
+* Contributors: Hadi Tabatabaee, Joshua Whitley
+
+0.0.2 (2020-03-04)
+------------------
+* Include signal messages in README
+* add messages for ground truth signals
+* updated license
+* Contributors: David Uhm
+
+0.0.1 (2018-12-18)
+------------------
+* add changelog
+* update initial version number
+* add license
+* add Ros package for ground truth messages
+* initial commit
+* Contributors: David Uhm

src/ros/catkin/src/lgsvl_msgs-master/CMakeLists.txt

@@ -0,0 +1,104 @@
+project(lgsvl_msgs)
+
+find_package(ros_environment REQUIRED)
+
+set(ROS_VERSION $ENV{ROS_VERSION})
+
+set(MSG_FILES
+  "BoundingBox2D.msg"
+  "BoundingBox3D.msg"
+  "CanBusData.msg"
+  "DetectedRadarObjectArray.msg"
+  "DetectedRadarObject.msg"
+  "Detection2D.msg"
+  "Detection2DArray.msg"
+  "Detection3D.msg"
+  "Detection3DArray.msg"
+  "Signal.msg"
+  "SignalArray.msg"
+  "Ultrasonic.msg"
+  "VehicleControlData.msg"
+  "VehicleStateData.msg"
+  "VehicleOdometry.msg"
+)
+
+if(${ROS_VERSION} EQUAL 1)
+  cmake_minimum_required(VERSION 2.8.3)
+
+  find_package(catkin REQUIRED COMPONENTS
+    geometry_msgs
+    message_generation
+    sensor_msgs
+    std_msgs
+  )
+
+  # Default to C++11
+  if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 11)
+  endif()
+
+  add_message_files(
+    DIRECTORY msg
+    FILES ${MSG_FILES}
+  )
+
+  generate_messages(
+    DEPENDENCIES
+    geometry_msgs
+    sensor_msgs
+    std_msgs
+  )
+
+  catkin_package(
+    CATKIN_DEPENDS
+    message_runtime
+    geometry_msgs
+    sensor_msgs
+    std_msgs
+  )
+elseif(${ROS_VERSION} EQUAL 2)
+  cmake_minimum_required(VERSION 3.5)
+
+  find_package(ament_cmake REQUIRED)
+  find_package(builtin_interfaces REQUIRED)
+  find_package(geometry_msgs REQUIRED)
+  find_package(sensor_msgs REQUIRED)
+  find_package(std_msgs REQUIRED)
+  find_package(rosidl_default_generators REQUIRED)
+
+  # Default to C++14
+  if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD_REQUIRED ON)
+    set(CMAKE_CXX_STANDARD 14)
+  endif()
+
+  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+  endif()
+
+  # Apend "msg/" to each file name
+  set(TEMP_LIST "")
+  foreach(MSG_FILE ${MSG_FILES})
+    list(APPEND TEMP_LIST "msg/${MSG_FILE}")
+  endforeach()
+  set(MSG_FILES ${TEMP_LIST})
+
+  rosidl_generate_interfaces(${PROJECT_NAME}
+    ${MSG_FILES}
+    DEPENDENCIES
+      builtin_interfaces
+      geometry_msgs
+      sensor_msgs
+      std_msgs
+    ADD_LINTER_TESTS
+  )
+
+  ament_export_dependencies(rosidl_default_runtime)
+
+  if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+  endif()
+
+  ament_package()
+endif()

src/ros/catkin/src/lgsvl_msgs-master/LICENSE

@@ -0,0 +1,27 @@
+Copyright (c) 2019-2020, LG Electronics
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of LG Electronics nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/ros/catkin/src/lgsvl_msgs-master/README.md

@@ -0,0 +1,25 @@
+# ROS Package lgsvl_msgs for LG SVL Automotive Simulator
+
+This repository contains ROS message definitions for lgsvl_msgs to subscribe ROS messages being published by LG SVL Automotive Simulator via rosbridge.
+
+```text
+  - Detection3DArray.msg    # A list of 3D detections
+  - Detection3D.msg         # 3D detection including id, label, score, and 3D bounding box
+  - BoundingBox3D.msg       # A 3D bounding box definition
+  - Detection2DArray.msg    # A list of 2D detections
+  - Detection2D.msg         # 2D detection including id, label, score, and 2D bounding box
+  - BoundingBox2D.msg       # A 2D bounding box definition
+  - SignalArray.msg         # A list of traffic light detections
+  - Signal.msg              # 3D detection of a traffic light including id, label, score, and 3D bounding box
+  - CanBusData.msg          # Can bus data for an ego vehicle published by the simulator
+  - VehicleControlData.msg  # Vehicle control commands that the simulator subscribes to
+  - VehicleStateData.msg    # Description of the full state of an ego vehicle
+  - Ultrasonic.msg          # Minimum detected distance by an ultrasonic sensor
+  - VehicleOdometry.msg     # Odometry for an ego vehicle
+```
+
+## Copyright and License
+
+Copyright (c) 2018-2020 LG Electronics, Inc.
+
+This software contains code licensed as described in LICENSE.

src/ros/catkin/src/lgsvl_msgs-master/msg/BoundingBox2D.msg

@@ -0,0 +1,4 @@
+float32 x  # x position of the bounding box center in camera image space, in pixels
+float32 y  # y position of the bounding box center in camera image space, in pixels
+float32 width  # width of the bounding box, in pixels
+float32 height  # height of the bounding box, in pixels

src/ros/catkin/src/lgsvl_msgs-master/msg/BoundingBox3D.msg

@@ -0,0 +1,2 @@
+geometry_msgs/Pose position  # 3D position and orientation of the bounding box center in Lidar space, in meters
+geometry_msgs/Vector3 size  # Size of the bounding box, in meters

src/ros/catkin/src/lgsvl_msgs-master/msg/CanBusData.msg

@@ -0,0 +1,29 @@
+std_msgs/Header header
+
+float32 speed_mps
+float32 throttle_pct  # 0 to 1
+float32 brake_pct     # 0 to 1
+float32 steer_pct     # -1 to 1
+bool parking_brake_active
+bool high_beams_active
+bool low_beams_active
+bool hazard_lights_active
+bool fog_lights_active
+bool left_turn_signal_active
+bool right_turn_signal_active
+bool wipers_active
+bool reverse_gear_active
+int8 selected_gear
+bool engine_active
+float32 engine_rpm
+float64 gps_latitude
+float64 gps_longitude
+float64 gps_altitude
+geometry_msgs/Quaternion orientation
+geometry_msgs/Vector3 linear_velocities
+
+int8 GEAR_NEUTRAL = 0
+int8 GEAR_DRIVE = 1
+int8 GEAR_REVERSE = 2
+int8 GEAR_PARKING = 3
+int8 GEAR_LOW = 4

src/ros/catkin/src/lgsvl_msgs-master/msg/DetectedRadarObject.msg

@@ -0,0 +1,18 @@
+int32 id
+
+geometry_msgs/Vector3 sensor_aim
+geometry_msgs/Vector3 sensor_right
+geometry_msgs/Point sensor_position
+geometry_msgs/Vector3 sensor_velocity
+float64 sensor_angle
+
+geometry_msgs/Point object_position
+geometry_msgs/Vector3 object_velocity
+geometry_msgs/Point object_relative_position
+geometry_msgs/Vector3 object_relative_velocity
+geometry_msgs/Vector3 object_collider_size
+uint8 object_state
+bool new_detection
+
+uint8 STATE_MOVING = 0
+uint8 STATE_STATIONARY = 1

src/ros/catkin/src/lgsvl_msgs-master/msg/DetectedRadarObjectArray.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+
+lgsvl_msgs/DetectedRadarObject[] objects

src/ros/catkin/src/lgsvl_msgs-master/msg/Detection2D.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+uint32 id  # The numeric ID of the detected object
+string label  # car, pedestrian
+float32 score  # The confidence score of the detected object in the range [0-1]
+BoundingBox2D bbox  # A 2D bounding box
+geometry_msgs/Twist velocity  # Linear and angular velocity

src/ros/catkin/src/lgsvl_msgs-master/msg/Detection2DArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+Detection2D[] detections  # A list of 2D detections

src/ros/catkin/src/lgsvl_msgs-master/msg/Detection3D.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+uint32 id  # The numeric ID of the detected object
+string label  # car, pedestrian
+float32 score  # The confidence score of the detected object in the range [0-1]
+BoundingBox3D bbox  # A 3D bounding box
+geometry_msgs/Twist velocity  # Linear and angular velocity

src/ros/catkin/src/lgsvl_msgs-master/msg/Detection3DArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+Detection3D[] detections  # A list of 3D detections

src/ros/catkin/src/lgsvl_msgs-master/msg/Signal.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+uint32 id  # The numeric ID of the detected signal
+string label  # green, yellow, red
+float32 score  # The confidence score of the detected signal in the range [0-1]
+BoundingBox3D bbox  # A 3D bounding box

src/ros/catkin/src/lgsvl_msgs-master/msg/SignalArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+Signal[] signals  # A list of traffic signals

src/ros/catkin/src/lgsvl_msgs-master/msg/Ultrasonic.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+float32 minimum_distance

src/ros/catkin/src/lgsvl_msgs-master/msg/VehicleControlData.msg

@@ -0,0 +1,13 @@
+std_msgs/Header header
+
+float32 acceleration_pct  # 0 to 1
+float32 braking_pct  # 0 to 1
+float32 target_wheel_angle  # radians
+float32 target_wheel_angular_rate  # radians / second
+uint8 target_gear
+
+uint8 GEAR_NEUTRAL = 0
+uint8 GEAR_DRIVE = 1
+uint8 GEAR_REVERSE = 2
+uint8 GEAR_PARKING = 3
+uint8 GEAR_LOW = 4

src/ros/catkin/src/lgsvl_msgs-master/msg/VehicleOdometry.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+float32 velocity # meters / second 
+float32 front_wheel_angle # radians
+float32 rear_wheel_angle # radians

src/ros/catkin/src/lgsvl_msgs-master/msg/VehicleStateData.msg

@@ -0,0 +1,36 @@
+std_msgs/Header header
+
+uint8 blinker_state
+uint8 headlight_state
+uint8 wiper_state
+uint8 current_gear
+uint8 vehicle_mode
+bool hand_brake_active
+bool horn_active
+bool autonomous_mode_active
+
+uint8 BLINKERS_OFF = 0
+uint8 BLINKERS_LEFT = 1
+uint8 BLINKERS_RIGHT = 2
+uint8 BLINKERS_HAZARD = 3
+
+uint8 HEADLIGHTS_OFF = 0
+uint8 HEADLIGHTS_LOW = 1
+uint8 HEADLIGHTS_HIGH = 2
+
+uint8 WIPERS_OFF = 0
+uint8 WIPERS_LOW = 1
+uint8 WIPERS_MED = 2
+uint8 WIPERS_HIGH = 3
+
+uint8 GEAR_NEUTRAL = 0
+uint8 GEAR_DRIVE = 1
+uint8 GEAR_REVERSE = 2
+uint8 GEAR_PARKING = 3
+uint8 GEAR_LOW = 4
+
+uint8 VEHICLE_MODE_COMPLETE_MANUAL = 0
+uint8 VEHICLE_MODE_COMPLETE_AUTO_DRIVE = 1
+uint8 VEHICLE_MODE_AUTO_STEER_ONLY = 2
+uint8 VEHICLE_MODE_AUTO_SPEED_ONLY = 3
+uint8 VEHICLE_MODE_EMERGENCY_MODE = 4

src/ros/catkin/src/lgsvl_msgs-master/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lgsvl_msgs</name>
+  <version>0.0.4</version>
+  <description>Message definitions for interfacing with the LGSVL Simulator for ROS and ROS 2.</description>
+  <maintainer email="hadi.tabatabaee@lge.com">Hadi Tabatabaee</maintainer>
+  <maintainer email="david.uhm@lge.com">David Uhm</maintainer>
+  <license>BSD</license>
+  <author email="david.uhm@lge.com">David Uhm</author>
+
+  <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
+
+  <build_depend condition="$ROS_VERSION == 1">message_generation</build_depend>
+  <build_depend condition="$ROS_VERSION == 2">rosidl_default_generators</build_depend>
+  <build_depend>ros_environment</build_depend>
+
+  <depend condition="$ROS_VERSION == 2">builtin_interfaces</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <exec_depend condition="$ROS_VERSION == 1">message_runtime</exec_depend>
+  <exec_depend condition="$ROS_VERSION == 2">rosidl_default_runtime</exec_depend>
+
+  <test_depend condition="$ROS_VERSION == 2">ament_lint_auto</test_depend>
+  <test_depend condition="$ROS_VERSION == 2">ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type condition="$ROS_VERSION == 1">catkin</build_type>
+    <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+  </export>
+</package>

src/ros/catkin/src/rostopilot/CMakeLists.txt

@@ -42,6 +42,11 @@ if (MODULECOMM_INCLUDE_DIR  AND  MODULECOMM_LIBRARAY_DIR )
 endif ()
 
 
+
+find_package(autoware_msgs REQUIRED)
+find_package(lgsvl_msgs REQUIRED)
+
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -53,6 +58,8 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   pcl_ros
   pcl_conversions
+  autoware_msgs
+  lgsvl_msgs
 )
 find_package(Boost REQUIRED)
 find_package(OpenCV REQUIRED)
@@ -116,9 +123,11 @@ catkin_package(
 include_directories(
 ##  INCLUDE_DIRS include
   ${CMAKE_CURRENT_BINARY_DIR}/..
-  ${CMAKE_SOURCE_DIR}/../devel/include
+ # ${CMAKE_SOURCE_DIR}/../devel/include
   ${catkin_INCLUDE_DIRS} include
   ${Boost_INCLUDE_DIR}
+  ${autoware_msgs_INCLUDE_DIRS}
+  ${lgsvl_msgs_INCLUDE_DIRS}
 )
 
 

src/ros/catkin/src/rostopilot/src/main.cpp

@@ -19,6 +19,8 @@
 #include <nav_msgs/Odometry.h>
 
 #include  "autoware_msgs/VehicleCmd.h"
+#include "lgsvl_msgs/VehicleControlData.h"
+#include "lgsvl_msgs/VehicleStateData.h"
 
 #include "rawpic.pb.h"
 #include "odom.pb.h"
@@ -43,6 +45,8 @@ static std::string _odom_msgname = "odom_ros";
 
 
 ros::Publisher  test_cmd_pub;
+ros::Publisher lgsvl_state_pub;
+ros::Publisher lgsvl_cmd_pub;
 
 
 static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
@@ -179,20 +183,17 @@ void testvh()
      int x = 0;
      while(1)
      {
-         autoware_msgs::VehicleCmd xcmd;
-
-         xcmd.header.seq = x;
 
-
-         xcmd.ctrl_cmd.linear_acceleration =0.1;
-         xcmd.ctrl_cmd.linear_velocity = 30.0;
-         xcmd.ctrl_cmd.steering_angle =0.0; 
-        xcmd.gear_cmd.gear = xcmd.gear_cmd.DRIVE;
-         xcmd.lamp_cmd.l =  0;
-         xcmd.lamp_cmd.r = 0;
-
-
-         test_cmd_pub.publish(xcmd);
+         lgsvl_msgs::VehicleStateData xstate;
+         xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_LEFT;
+         xstate.headlight_state = lgsvl_msgs::VehicleStateData::HEADLIGHTS_OFF;
+         xstate.wiper_state = lgsvl_msgs::VehicleStateData::WIPERS_HIGH;
+         xstate.current_gear = lgsvl_msgs::VehicleStateData::GEAR_PARKING;
+         xstate.vehicle_mode = lgsvl_msgs::VehicleStateData::VEHICLE_MODE_COMPLETE_AUTO_DRIVE;
+         xstate.hand_brake_active = true;
+         xstate.horn_active = true;
+         xstate.autonomous_mode_active = true;
+         lgsvl_state_pub.publish(xstate);
          x++;
          std::this_thread::sleep_for(std::chrono::milliseconds(10));
      }
@@ -209,41 +210,58 @@ void UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned
         std::cout<<" UpdateDecition Parse error."<<std::endl;
         return;
     }
-    //ServiceCarStatus.mfAcc = xdecition.accelerator();   //
+    /*
          autoware_msgs::VehicleCmd xcmd;
-
-         xcmd.header.seq = x;
-
-
-        std::cout<<" acc : "<<xdecition.accelerator()<<std::endl;
-        //xcmd.ctrl_cmd.linear_acceleration =0.1;
           xcmd.twist_cmd.twist.linear.x =   xdecition.speed()/3.6;
           xcmd.twist_cmd.twist.angular.z = 3.0* xdecition.wheelangle() /600.0; 
          xcmd.ctrl_cmd.linear_velocity = 30.0;
-         //xcmd.ctrl_cmd.steering_angle = xdecition.wheelangle() * (-1.0)/600.0; 
-        //xcmd.gear_cmd.gear =  1;//autoware_msgs::VehicleCmd::_gear_cmd_type::NONE;
-        xcmd.mode = 1;
-        if(xdecition.leftlamp()  == true)
-        {
-            xcmd.lamp_cmd.l = 1;
-        }
-        else
-        {
-            xcmd.lamp_cmd.l = 0;
-        }
+         test_cmd_pub.publish(xcmd);
+    */
+   lgsvl_msgs::VehicleStateData xstate;
+   lgsvl_msgs::VehicleControlData xctrl;
+    if(xdecition.leftlamp() == true)
+    {
+        xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_LEFT;
+    }
+    else
+    {
         if(xdecition.rightlamp() == true)
-        {
-            xcmd.lamp_cmd.r = 1;
+        { 
+            xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_RIGHT;
         }
         else
-        {
-            xcmd.lamp_cmd.r = 0;
-        }
-        // xcmd.lamp_cmd.l =  0;
-         //xcmd.lamp_cmd.r = 0;
+            xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_OFF;
+    }
+         xstate.headlight_state = lgsvl_msgs::VehicleStateData::HEADLIGHTS_OFF;
+         xstate.wiper_state = lgsvl_msgs::VehicleStateData::WIPERS_OFF;
+         xstate.current_gear = lgsvl_msgs::VehicleStateData::GEAR_DRIVE;
+         xstate.vehicle_mode = lgsvl_msgs::VehicleStateData::VEHICLE_MODE_COMPLETE_AUTO_DRIVE;
+         xstate.hand_brake_active = false;
+         xstate.horn_active = true;
+         xstate.autonomous_mode_active = true;
+    
+    if(xdecition.brake() < 0.00001)
+    {
+        xctrl.acceleration_pct = xdecition.accelerator() /5.0;
+        if(xctrl.acceleration_pct >1.0)xctrl.acceleration_pct = 1.0;
+        if(xctrl.acceleration_pct<0.0)xctrl.acceleration_pct = 0.0;
+        xctrl.braking_pct = 0.0;
+    }
+    else
+    {
+        xctrl.acceleration_pct = 0.0;
+        xctrl.braking_pct = xdecition.brake()/100.0;
+    }
 
+    //        xctrl.acceleration_pct = 0.05;
+     //   xctrl.braking_pct = 0;
+    xctrl.target_gear =  lgsvl_msgs::VehicleControlData::GEAR_DRIVE;
+    xctrl.target_wheel_angle = 0.1 *  (-1)* xdecition.wheelangle() *M_PI/180.0;
+    xctrl.target_wheel_angular_rate = 300 * M_PI/180.0;
+
+    lgsvl_state_pub.publish(xstate);
+    lgsvl_cmd_pub.publish(xctrl);
 
-         test_cmd_pub.publish(xcmd);
 }
 
 int main(int argc, char *argv[])
@@ -270,6 +288,8 @@ int main(int argc, char *argv[])
 
     test_cmd_pub =  private_nh.advertise<autoware_msgs::VehicleCmd>(
                 "/vehicle_cmd", 10);
+    lgsvl_state_pub = private_nh.advertise<lgsvl_msgs::VehicleStateData>("/vehicle_state",10);
+    lgsvl_cmd_pub = private_nh.advertise<lgsvl_msgs::VehicleControlData>("/vehicle_ctrl",10);
 
 	gpa_lidar = iv::modulecomm::RegisterSend(_point_msgname.data(),20000000,1);
     gpa_image = iv::modulecomm::RegisterSend(_image_msgname.data(),20000000,1);
@@ -282,8 +302,8 @@ int main(int argc, char *argv[])
 
     odom_sub = private_nh.subscribe(_odom_topic,1,odomCalllback);
 
-    void * pdecition = iv::modulecomm::RegisterRecv("deciton",UpdateDecition);
- //   std::thread * pnew = new std::thread(testvh);
+   void * pdecition = iv::modulecomm::RegisterRecv("deciton",UpdateDecition);
+  //   std::thread * pnew = new std::thread(testvh);
 
    ros::spin();
    /*