diff --git a/Drive System/sub joynode pub roverdrive b/Drive System/sub joynode pub roverdrive new file mode 100644 index 0000000..d671798 --- /dev/null +++ b/Drive System/sub joynode pub roverdrive @@ -0,0 +1,106 @@ +#!/usr/bin/env python +import rospy +from sensor_msgs.msg import Joy +from geometry_msgs.msg import Point +from numpy import interp + + + #if turned on the direction is reversed + +pwm=[0,0,0] + +point = Point() + + +def find_pwmtest3(msg): + '''dir_n = 0 + + pwm[2] = dir_n + pwmtotal=interp(msg.axes[2],[-1,1],[-255,255]) + + if (pwmtotal < 0): + dir_n = 1 + pwm[2] = dir_n + pwmtotal=pwmtotal*-1 + if (msg.buttons[4] == 1): + pwm[0]=pwmtotal + if (msg.buttons[4] == 0): + pwm[0]=0 + if (msg.buttons[5] == 1): + pwm[1]=pwmtotal + if (msg.buttons[5] == 0): + pwm[1]=0 + ''' + pwmtotal=interp(msg.axes[1],[-1,1],[-255,255]) + pwmdir=interp(msg.axes[2],[-1,1],[-155,155]) + pwm[2]=0 + if(pwmtotal<0): + pwm[2]=1 + pwmtotal=pwmtotal*-1 + if(pwmdir>=0): + pwm[0]=pwmtotal-pwmdir + pwm[1]=0 + if(pwmdir<0): + pwm[0]=0 + pwm[1]=pwmtotal+pwmdir + #rospy.loginfo("left pwm %f",pwm[0]) + #rospy.loginfo("right pwm %f",pwm[1]) + rospy.loginfo(pwm) + +def find_pwmtest2(msg): + dir_n = 0 + + pwm[2] = dir_n + pwmtotal=interp(msg.axes[2],[-1,1],[-255,255]) + + if (pwmtotal < 0): + dir_n = 1 + pwm[2] = dir_n + pwmtotal=pwmtotal*-1 + if (msg.buttons[4] == 1): + pwm[0]=pwmtotal + if (msg.buttons[4] == 0): + pwm[0]=0 + if (msg.buttons[5] == 1): + pwm[1]=pwmtotal + if (msg.buttons[5] == 0): + pwm[1]=0 + + #rospy.loginfo("left pwm %f",pwm[0]) + #rospy.loginfo("right pwm %f",pwm[1]) + rospy.loginfo(pwm) + + +def find_pwmtest1(msg): #1st case when 2 pwm valus are send for left ans right seperately + pwm=[0,0] + valL= msg.axes[1] + valR= msg.axes[2] + pwm[0]=interp(valL,[0,1],[0,255]) + pwm[1]=interp(valR,[0,1],[0,255]) + rospy.loginfo("left pwm %f",pwm[0]) + rospy.loginfo("right pwm %f",pwm[1]) + +def transform(a): + point.x = pwm[0] + point.y = pwm[1] + point.z = pwm[2] + +def callback(msg): + rate = rospy.Rate(100) + find_pwmtest3(msg) + point.x = pwm[0] + point.y = pwm[1] + point.z = pwm[2] + pub = rospy.Publisher('rover_drive', Point ,queue_size=100) + pub.publish(point) + rate.sleep() + #rospy.loginfo("I heard %f",msg.buttons[]) + + +def listener(): + rospy.init_node('listener_joy', anonymous=True) + rospy.Subscriber("joy", Joy, callback) + rospy.spin() + +if __name__ == '__main__': + listener() diff --git a/Drive System/sub roverdrive and sending serialcom to arduino b/Drive System/sub roverdrive and sending serialcom to arduino new file mode 100644 index 0000000..d4742e8 --- /dev/null +++ b/Drive System/sub roverdrive and sending serialcom to arduino @@ -0,0 +1,44 @@ +#!/usr/bin/env python +import rospy +from geometry_msgs.msg import Point +import serial +import time + +arduino = serial.Serial('/dev/ttyACM0', 9600, timeout=.1) + + +def callback(data): + valL=data.x + if (valL == 0.0): + valL=str("000") + else: + valL=str(int(valL)) + + valR=data.y + if (valR == 0.0): + valR=str("000") + else: + valR=str(int(valR)) + + valD=data.z + if (valD == 0.0): + valD=str("0") + else: + valD=str(int(valD)) + + val= valL + " " + valR + " " + valD + " " + '\0' + + rospy.loginfo(val) + arduino.write(val) + +def listener(): + + rospy.init_node('pwmreciver', anonymous=True) + + rospy.Subscriber("rover_drive", Point , callback) + + rospy.spin() + +if __name__ == '__main__': + listener() + diff --git a/arduino code for serialcom b/arduino code for serialcom new file mode 100644 index 0000000..5b6f603 --- /dev/null +++ b/arduino code for serialcom @@ -0,0 +1,56 @@ + +int data; +long unsigned int totaldata; +char dataL[4],dataR[4], dataD[4]; +void setup() { + Serial.begin(9600); + pinMode(9,OUTPUT); + pinMode(8,OUTPUT); + pinMode(10,OUTPUT); +} +void loop() { + if(Serial.available() > 0) { + char buffer[] = {' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' '}; // Receive up to 10 bytes + while (!Serial.available()); // Wait for characters + Serial.readBytesUntil('\0', buffer, 14); + int x=0,i=0,j=0,y=0,val3=0; + while(buffer[i]!=' ') + { + dataL[x]=buffer[i]; + i++; + x++; + } + dataL[x]='\0'; + i++; + + while(buffer[i]!=' ') + { + dataR[j]=buffer[i]; + i++; + j++; + } + dataR[j]='\0'; + i++; + if(buffer[i]=='1') + val3=1; + + int val1 = atoi(dataL); + int val2 = atoi(dataR); + + //dataD=totaldata%1000; + //totaldata + + val1=(int(val1/10))*10; + val2=(int(val2/10))*10; + + Serial.print(val1); + Serial.print(","); + Serial.print(val2); + Serial.print("'"); + Serial.println(val3); + analogWrite(9,val1); + analogWrite(10,val2); + digitalWrite(8,val3); + } + +} diff --git a/drive/CMakeLists.txt b/drive/CMakeLists.txt new file mode 100644 index 0000000..4834c62 --- /dev/null +++ b/drive/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 2.8.3) +project(drive) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + message_generation +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + drive_msg.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES drive + CATKIN_DEPENDS roscpp rospy std_msgs message_runtime + 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} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/drive.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/drive_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_drive.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/drive/msg/drive_msg.msg b/drive/msg/drive_msg.msg new file mode 100755 index 0000000..2be1b25 --- /dev/null +++ b/drive/msg/drive_msg.msg @@ -0,0 +1,4 @@ +float32 lpwm +float32 rpwm +bool ldir +bool rdir diff --git a/drive/package.xml b/drive/package.xml new file mode 100644 index 0000000..8bfe2a6 --- /dev/null +++ b/drive/package.xml @@ -0,0 +1,70 @@ + + + drive + 0.0.0 + The drive package + + + + + divyanshu + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + message_generation + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + message_runtime + + + + + + + + diff --git a/drive/scripts/arduino_code.ino b/drive/scripts/arduino_code.ino new file mode 100644 index 0000000..58a01c7 --- /dev/null +++ b/drive/scripts/arduino_code.ino @@ -0,0 +1,62 @@ +int data; +long unsigned int totaldata; +char dataL[4],dataR[4], dataD[4]; +void setup() { + Serial.begin(9600); + pinMode(9,OUTPUT); + pinMode(8,OUTPUT); + pinMode(10,OUTPUT); + pinMode(7,OUTPUT); +} +void loop() { + if(Serial.available() > 0) { + char buffer[] = {' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' ',' '}; // Receive up to 1 bytes + while (!Serial.available()); // Wait for characters + Serial.readBytesUntil('\0', buffer, 19); + int x=0,i=0,j=0,y=0,val3=0,val4=0; + while(buffer[i]!=' ') + { + dataL[x]=buffer[i]; + i++; + x++; + } + dataL[x]='\0'; + i++; + + while(buffer[i]!=' ') + { + dataR[j]=buffer[i]; + i++; + j++; + } + dataR[j]='\0'; + i++; + if(buffer[i]=='1') + val3=1; + i=i+2; + if(buffer[i]=='1') + val4=1; + + int val1 = atoi(dataL); + int val2 = atoi(dataR); + + //dataD=totaldata%1000; + //totaldata + + val1=(int(val1/10))*10; + val2=(int(val2/10))*10; + + Serial.print(val1); + Serial.print(","); + Serial.print(val2); + Serial.print("'"); + Serial.print(val3); + Serial.print("'"); + Serial.println(val4); + analogWrite(9,val1); + analogWrite(10,val2); + digitalWrite(5,val3); + digitalWrite(6,val4); + } + +} diff --git a/drive/scripts/sub_joynode_pub_roverdrive.py b/drive/scripts/sub_joynode_pub_roverdrive.py new file mode 100755 index 0000000..c889704 --- /dev/null +++ b/drive/scripts/sub_joynode_pub_roverdrive.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python +import rospy +from sensor_msgs.msg import Joy +from drive.msg import drive_msg +from numpy import interp + + + #if turned on the direction is reversed + +pwm=[0,0,0,0] + +drivex=drive_msg() + +def find_pwmtest3(msg): + '''dir_n = 0 + + pwm[2] = dir_n + pwmtotal=interp(msg.axes[2],[-1,1],[-255,255]) + + if (pwmtotal < 0): + dir_n = 1 + pwm[2] = dir_n + pwmtotal=pwmtotal*-1 + if (msg.buttons[4] == 1): + pwm[0]=pwmtotal + if (msg.buttons[4] == 0): + pwm[0]=0 + if (msg.buttons[5] == 1): + pwm[1]=pwmtotal + if (msg.buttons[5] == 0): + pwm[1]=0 + ''' + if(msg.buttons[6]==0): + pwndir=0 + pwmtotal=interp(msg.axes[1],[-1,1],[-255,255]) + pwmdir=interp(msg.axes[3],[-1,1],[-155,155]) + #rospy.loginfo(pwmdir) + #rospy.loginfo(pwmtotal) + pwm[2]=0 + pwm[3]=0 + if(pwmtotal<0): + pwm[2]=1 + pwm[3]=1 + pwmtotal=pwmtotal*-1 + if(pwmdir>=0): + pwm[0] = pwmtotal - pwmdir + pwm[1]= pwmtotal + if(pwmdir<0): + pwm[0]= pwmtotal + pwm[1]= pwmtotal + pwmdir + if(msg.buttons[4]==1): + pwm[0]=200 + pwm[1]=200 + pwm[2]=1 + pwm[3]=0 + if(msg.buttons[5]==1): + pwm[0]=200 + pwm[1]=200 + pwm[2]=0 + pwm[3]=1 + else: + pwm[0]=0 + pwm[1]=0 + pwm[2]=0 + pwm[3]=0 + + #rospy.loginfo("left pwm %f",pwm[0]) + #rospy.loginfo("right pwm %f",pwm[1]) + rospy.loginfo(pwm) + +def find_pwmtest2(msg): + dir_n = 0 + + pwm[2] = dir_n + pwmtotal=interp(msg.axes[2],[-1,1],[-255,255]) + + if (pwmtotal < 0): + dir_n = 1 + pwm[2] = dir_n + pwmtotal=pwmtotal*-1 + if (msg.buttons[4] == 1): + pwm[0]=pwmtotal + if (msg.buttons[4] == 0): + pwm[0]=0 + if (msg.buttons[5] == 1): + pwm[1]=pwmtotal + if (msg.buttons[5] == 0): + pwm[1]=0 + + #rospy.loginfo("left pwm %f",pwm[0]) + #rospy.loginfo("right pwm %f",pwm[1]) + rospy.loginfo(pwm) + + +def find_pwmtest1(msg): #1st case when 2 pwm valus are send for left ans right seperately + pwm=[0,0] + valL= msg.axes[1] + valR= msg.axes[2] + pwm[0]=interp(valL,[0,1],[0,255]) + pwm[1]=interp(valR,[0,1],[0,255]) + rospy.loginfo("left pwm %f",pwm[0]) + rospy.loginfo("right pwm %f",pwm[1]) + +#def transform(a): +# point.x = pwm[0] +# point.y = pwm[1] +# point.z = pwm[2] + +def callback(msg): + rate = rospy.Rate(100) + find_pwmtest3(msg) + drivex.lpwm = pwm[0] + drivex.rpwm = pwm[1] + drivex.ldir = bool(pwm[2]) + drivex.rdir = bool(pwm[3]) + pub = rospy.Publisher('rover_drive', drive_msg ,queue_size=10) + pub.publish(drivex) + rate.sleep() + #rospy.loginfo("I heard %f",msg.buttons[]) + + +def listener(): + rospy.init_node('listener_joy', anonymous=True) + rospy.Subscriber("joy", Joy, callback) + rospy.spin() + +if __name__ == '__main__': + listener() diff --git a/drive/scripts/sub_roverdrive.py b/drive/scripts/sub_roverdrive.py new file mode 100755 index 0000000..5f1b63a --- /dev/null +++ b/drive/scripts/sub_roverdrive.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python +import rospy +from drive.msg import drive_msg +import serial +import time + +arduino = serial.Serial('/dev/ttyACM0', 9600, timeout=.1) + + +def callback(data): + valL=data.lpwm + if (valL == 0.0): + valL=str("000") + else: + valL=str(int(valL)) + + valR=data.rpwm + if (valR == 0.0): + valR=str("000") + else: + valR=str(int(valR)) + + LvalD=data.ldir + if (LvalD == 0): + LvalD=str("0") + else: + LvalD=str("1") + + RvalD=data.rdir + if (RvalD == 0): + RvalD=str("0") + else: + RvalD=str("1") + + val= valL + " " + valR + " " + LvalD + " " + RvalD + '\0' + + rospy.loginfo("0123456789") + rospy.loginfo(val) + arduino.write(val) + +def listener(): + #data.lpwm=0 + #data.rpwm=0 + #data.ldir=0 + #data.rdir=0 + rospy.init_node('pwmreciver', anonymous=True) + + rospy.Subscriber("rover_drive", drive_msg , callback) + + rospy.spin() + +if __name__ == '__main__': + listener() + diff --git a/extract_msg/CMakeLists.txt b/extract_msg/CMakeLists.txt new file mode 100644 index 0000000..bc9714a --- /dev/null +++ b/extract_msg/CMakeLists.txt @@ -0,0 +1,197 @@ +cmake_minimum_required(VERSION 2.8.3) +project(extract_msg) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES extract_msg +# CATKIN_DEPENDS rospy +# 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} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/extract_msg.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/extract_msg_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_extract_msg.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/extract_msg/package.xml b/extract_msg/package.xml new file mode 100644 index 0000000..2cd2e7a --- /dev/null +++ b/extract_msg/package.xml @@ -0,0 +1,62 @@ + + + extract_msg + 0.0.0 + The extract_msg package + + + + + divyanshu + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + rospy + rospy + + + + + + + + diff --git a/extract_msg/src/Untitled Document b/extract_msg/src/Untitled Document new file mode 100644 index 0000000..ae6ef25 --- /dev/null +++ b/extract_msg/src/Untitled Document @@ -0,0 +1,52 @@ +#!/usr/bin/env python +from collections import deque +import numpy as np +import argparse +import imutils +import cv2 + +ap = argparse.ArgumentParser() +ap.add_argument("-v", "--video", help="path to the (optional) video file") +args = vars(ap.parse_args()) + +greenLower = (29, 86, 6) +greenUpper = (64, 255, 255) + +if not args.get("video", False): + camera = cv2.VideoCapture(0) +else: + camera = cv2.VideoCapture(args["video"]) + +while True: + (grabbed, frame) = camera.read() + + if args.get("video") and not grabbed: + break + + frame = imutils.resize(frame, width=600) + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + + mask = cv2.inRange(hsv, greenLower, greenUpper) + mask = cv2.erode(mask, None, iterations=2) + mask = cv2.dilate(mask, None, iterations=2) + + cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] + center = None + + if len(cnts) > 0: + c = max(cnts, key=cv2.contourArea) + ((x, y), radius) = cv2.minEnclosingCircle(c) + + if radius > 50: + cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2) + + cv2.imshow("Frame", frame) + cv2.imshow("Mask", mask) + + key = cv2.waitKey(1) & 0xFF + + if key == ord("q"): + break + +camera.release() +cv2.destroyAllWindows() diff --git a/extract_msg/src/adv_listnter.py b/extract_msg/src/adv_listnter.py new file mode 100755 index 0000000..bb52c14 --- /dev/null +++ b/extract_msg/src/adv_listnter.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python +import rospy +from sensor_msgs.msg import Joy +from geometry_msgs.msg import Point +from numpy import interp + + + #if turned on the direction is reversed + +pwm=[0,0,0] + +point = Point() + +def find_pwmtest2(msg): + dir_n = 0 + + pwm[2] = dir_n + pwmtotal=interp(msg.axes[2],[-1,1],[-255,255]) + + if (pwmtotal < 0): + dir_n = 1 + pwm[2] = dir_n + pwmtotal=pwmtotal*-1 + if (msg.buttons[4] == 1): + pwm[0]=pwmtotal + if (msg.buttons[4] == 0): + pwm[0]=0 + if (msg.buttons[5] == 1): + pwm[1]=pwmtotal + if (msg.buttons[5] == 0): + pwm[1]=0 + + #rospy.loginfo("left pwm %f",pwm[0]) + #rospy.loginfo("right pwm %f",pwm[1]) + rospy.loginfo(pwm) + + +def find_pwmtest1(msg): #1st case when 2 pwm valus are send for left ans right seperately + pwm=[0,0] + valL= msg.axes[1] + valR= msg.axes[2] + pwm[0]=interp(valL,[0,1],[0,255]) + pwm[1]=interp(valR,[0,1],[0,255]) + rospy.loginfo("left pwm %f",pwm[0]) + rospy.loginfo("right pwm %f",pwm[1]) + +def transform(): + point.x = pwm[0] + point.y = pwm[1] + point.z = pwm[2] + +def callback(msg): + rate = rospy.Rate(100) + find_pwmtest2(msg) + transform() + pub = rospy.Publisher('rover_drive', Point ,queue_size=100) + pub.publish(point) + rate.sleep() + #rospy.loginfo("I heard %f",msg.buttons[]) + + +def listener(): + rospy.init_node('listener_joy', anonymous=True) + rospy.Subscriber("joy", Joy, callback) + rospy.spin() + +if __name__ == '__main__': + listener() diff --git a/extract_msg/src/adv_listnter_test2.py b/extract_msg/src/adv_listnter_test2.py new file mode 100755 index 0000000..e346b8a --- /dev/null +++ b/extract_msg/src/adv_listnter_test2.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python +import rospy +from sensor_msgs.msg import Joy +from geometry_msgs.msg import Point +from numpy import interp + + + #if turned on the direction is reversed + +pwm=[0,0,0] + +point = Point() + +def find_pwmtest2(msg): + dir_n = 0 + + pwm[2] = dir_n + pwmtotal=interp(msg.axes[2],[-1,1],[-255,255]) + + if (pwmtotal < 0): + dir_n = 1 + pwm[2] = dir_n + pwmtotal=pwmtotal*-1 + if (msg.buttons[4] == 1): + pwm[0]=pwmtotal + if (msg.buttons[4] == 0): + pwm[0]=0 + if (msg.buttons[5] == 1): + pwm[1]=pwmtotal + if (msg.buttons[5] == 0): + pwm[1]=0 + + #rospy.loginfo("left pwm %f",pwm[0]) + #rospy.loginfo("right pwm %f",pwm[1]) + rospy.loginfo(pwm) + + +def find_pwmtest1(msg): #1st case when 2 pwm valus are send for left ans right seperately + pwm=[0,0] + valL= msg.axes[1] + valR= msg.axes[2] + pwm[0]=interp(valL,[0,1],[0,255]) + pwm[1]=interp(valR,[0,1],[0,255]) + rospy.loginfo("left pwm %f",pwm[0]) + rospy.loginfo("right pwm %f",pwm[1]) + + +def find_pwmtest3(msg): #1st case when 2 pwm valus are send for left ans right seperately + pwm=[0,0,0] + pwm[0]= msg.buttons[4] + pwm[1]= msg.buttons[5] + temp = msg.axes[1] + pwm[2]=interp(temp,[0,1],[0,255]) + #rospy.loginfo(" left direction %f",pwm[0]) + #rospy.loginfo("right direction %f",pwm[1]) + rospy.loginfo(pwm) + point.x = float(pwm[0]) + rospy.loginfo(" left direction %f",point.x) + point.y = float(pwm[1]) + point.z = float(pwm[2]) + # transform() + +def transform(): + point.x = float(pwm[0]) + rospy.loginfo(" left direction %f",point.x) + point.y = float(pwm[1]) + point.z = float(pwm[2]) + +def callback(msg): + rate = rospy.Rate(100) + find_pwmtest3(msg) + #transform() + pub = rospy.Publisher('rover_drive', Point ,queue_size=100) + pub.publish(point) + rate.sleep() + #rospy.loginfo("I heard %f",msg.buttons[]) + + +def listener(): + rospy.init_node('listener_joy', anonymous=True) + rospy.Subscriber("joy", Joy, callback) + rospy.spin() + +if __name__ == '__main__': + listener() diff --git a/extract_msg/src/adv_talker.py b/extract_msg/src/adv_talker.py new file mode 100755 index 0000000..e21e991 --- /dev/null +++ b/extract_msg/src/adv_talker.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python +import rospy +from geometry_msgs.msg import Point +import serial +import time + +arduino = serial.Serial('/dev/ttyACM1', 9600, timeout=.1) + + +def callback(data): + valL=data.x + if (valL == 0.0): + valL=str("000") + else: + valL=str(int(valL)) + + valR=data.y + if (valR == 0.0): + valR=str("000") + else: + valR=str(int(valR)) + + valD=data.z + if (valD == 0.0): + valD=str("0") + else: + valD=str(int(valD)) + + val= valL + " " + valR + " " + valD + " " + '\0' + + rospy.loginfo(val) + arduino.write(val) + +def listener(): + + rospy.init_node('pwmreciver', anonymous=True) + + rospy.Subscriber("rover_drive", Point , callback) + + rospy.spin() + +if __name__ == '__main__': + listener() + + diff --git a/extract_msg/src/colorpicker.py b/extract_msg/src/colorpicker.py new file mode 100755 index 0000000..ab2c465 --- /dev/null +++ b/extract_msg/src/colorpicker.py @@ -0,0 +1,83 @@ +import argparse +import cv2 +import imutils +import numpy as np + +colors = [] +lower = np.array([0, 0, 0]) +upper = np.array([0, 0, 0]) + +def on_mouse_click(event, x, y, flags, image): + if event == cv2.EVENT_LBUTTONUP: + colors.append(image[y,x].tolist()) + +def on_trackbar_change(position): + return + +# Parse command line arguments +parser = argparse.ArgumentParser() +parser.add_argument("-i", "--image", required=True, help="path to the image file") +parser.add_argument("-l", "--lower", help="HSV lower bounds") +parser.add_argument("-u", "--upper", help="HSV upper bounds") +args = vars(parser.parse_args()) + +# Parse lower and upper bounds arguments (--lower "1, 2, 3" --upper "4, 5, 6") +if args["lower"] and args["upper"]: + lower = np.fromstring(args["lower"], sep=",") + upper = np.fromstring(args["upper"], sep=",") + +# Load image, resize to 600 width, and convert color to HSV +image = cv2.imread(args["image"]) +image = imutils.resize(image, width=600) +hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + +# Create window +cv2.namedWindow('image') + +# Set mouse callback to capture HSV value on click +cv2.setMouseCallback("image", on_mouse_click, hsv) +cv2.createTrackbar("Min H", "image", int(lower[0]), 255, on_trackbar_change) +cv2.createTrackbar("Min S", "image", int(lower[1]), 255, on_trackbar_change) +cv2.createTrackbar("Min V", "image", int(lower[2]), 255, on_trackbar_change) +cv2.createTrackbar("Max H", "image", int(upper[0]), 255, on_trackbar_change) +cv2.createTrackbar("Max S", "image", int(upper[1]), 255, on_trackbar_change) +cv2.createTrackbar("Max V", "image", int(upper[2]), 255, on_trackbar_change) + +# Show HSV image +cv2.imshow("image", hsv) + +while True: + # Get trackbar positions and set lower/upper bounds + lower[0] = cv2.getTrackbarPos("Min H", "image") + lower[1] = cv2.getTrackbarPos("Min S", "image") + lower[2] = cv2.getTrackbarPos("Min V", "image") + upper[0] = cv2.getTrackbarPos("Max H", "image") + upper[1] = cv2.getTrackbarPos("Max S", "image") + upper[2] = cv2.getTrackbarPos("Max V", "image") + + # Create a range mask then erode and dilate to reduce noise + mask = cv2.inRange(hsv, lower, upper) + mask = cv2.erode(mask, None, iterations=2) + mask = cv2.dilate(mask, None, iterations=2) + + # Show range mask + cv2.imshow("mask", mask) + + # Press q to exit + if cv2.waitKey(1) & 0xFF == ord("q"): + break + +cv2.destroyAllWindows() + +if not colors: + exit + +minh = min(c[0] for c in colors) +mins = min(c[1] for c in colors) +minv = min(c[2] for c in colors) + +maxh = max(c[0] for c in colors) +maxs = max(c[1] for c in colors) +maxv = max(c[2] for c in colors) + +print [minh, mins, minv], [maxh, maxs, maxv] diff --git a/extract_msg/src/command.py b/extract_msg/src/command.py new file mode 100644 index 0000000..b3392fe --- /dev/null +++ b/extract_msg/src/command.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python +import rospy +from geometry_msgs.msg import Point + +def callback(data): + yaw = data.x + spin = data.y + roll = data.z + + +def start(): + + rospy.init_node('yawspinroll', anonymous=True) + + rospy.Subscriber("ysr_drive", Point , callback) + + rospy.spin() + +if __name__ == '__main__': + start() \ No newline at end of file diff --git a/extract_msg/src/gpsval.py b/extract_msg/src/gpsval.py new file mode 100755 index 0000000..e740ffd --- /dev/null +++ b/extract_msg/src/gpsval.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python +import rospy +import time +from geometry_msgs.msg import Point +import serial + +point = Point() + +arduino = serial.Serial('/dev/ttyUSB0',9600) + + +def talker(): + pub = rospy.Publisher('gpsval', Point, queue_size=100) + rospy.init_node('gps_sender', anonymous=True) + rate = rospy.Rate(100) + + # 10hz + while not rospy.is_shutdown(): + rawdata=[] + rawdata=str(arduino.readline()) + #rospy.loginfo(point) + if rawdata[0] == 'L': + lat=float(rawdata[4:14]) + lon=float(rawdata[18:29]) + point.x= lat + point.y= lon + point.z= 0 + print(lat) + print(lon) + pub.publish(point) + rate.sleep() + #rospy.spin() + +if __name__ == '__main__': + talker() + \ No newline at end of file diff --git a/extract_msg/src/gpsval_sub.py b/extract_msg/src/gpsval_sub.py new file mode 100755 index 0000000..5d69df3 --- /dev/null +++ b/extract_msg/src/gpsval_sub.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python +import rospy +from geometry_msgs.msg import Point + +def callback(data): + lat=data.x + lon=data.y + rospy.loginfo("latitude"+str(lat)) + rospy.loginfo("longitude"+str(lon)) + +def listener(): + + + rospy.init_node('getGPSval', anonymous=True) + + rospy.Subscriber("gpsval", Point, callback) + + # spin() simply keeps python from exiting until this node is stopped + rospy.spin() + +if __name__ == '__main__': + listener() diff --git a/extract_msg/src/practicecv2.py b/extract_msg/src/practicecv2.py new file mode 100755 index 0000000..2fb3457 --- /dev/null +++ b/extract_msg/src/practicecv2.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python + +import roslib +import sys +import rospy +import cv2 +import numpy as np +from cv_bridge import CvBridge, CvBridgeError +from geometry_msgs.msg import Twist +from sensor_msgs.msg import Image + + +class LineFollower(object): + + def __init__(self): + + self.bridge_object = CvBridge() + self.image_sub = rospy.Subscriber("/camera1/image_raw" , Image , self.camera_callback) + + def camera_callback(self,data): + + try: + # We select bgr8 because its the OpneCV encoding by default + cv_image = self.bridge_object.imgmsg_to_cv2(data, desired_encoding="bgr8") + except CvBridgeError as e: + print(e) + + cv2.imshow("Image window", cv_image) + cv2.waitKey(1) + + +def main(): + line_follower_object = LineFollower() + rospy.init_node('line_following_node', anonymous=True) + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") + cv2.destroyAllWindows() + +if __name__ == '__main__': + main() diff --git a/extract_msg/src/ros_video.py b/extract_msg/src/ros_video.py new file mode 100755 index 0000000..5e1b987 --- /dev/null +++ b/extract_msg/src/ros_video.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Copyright 2016 Massachusetts Institute of Technology + +"""Publish a video as ROS messages. +""" + +import argparse + +import numpy as np + +import cv2 + +import rospy + +from sensor_msgs.msg import Image +from sensor_msgs.msg import CameraInfo + +import camera_info_manager + +from cv_bridge import CvBridge, CvBridgeError + +bridge = CvBridge() + +def main(): + """Publish a video as ROS messages. + """ + # Patse arguments. + parser = argparse.ArgumentParser(description="Convert video into a rosbag.") + parser.add_argument("video_file", help="Input video.") + parser.add_argument("-c", "--camera", default="camera", help="Camera name.") + parser.add_argument("-f", "--frame_id", default="camera", + help="tf frame_id.") + parser.add_argument("--width", type=np.int32, default="640", + help="Image width.") + parser.add_argument("--height", type=np.int32, default="480", + help="Image height.") + parser.add_argument("--info_url", default="file:///camera.yml", + help="Camera calibration url.") + + args = parser.parse_args() + + print "Publishing %s." % (args.video_file) + + # Set up node. + rospy.init_node("video_publisher", anonymous=True) + img_pub = rospy.Publisher("/" + args.camera + "/image_raw", Image, + queue_size=10) + info_pub = rospy.Publisher("/" + args.camera + "/camera_info", CameraInfo, + queue_size=10) + + info_manager = camera_info_manager.CameraInfoManager(cname=args.camera, + url=args.info_url, + namespace=args.camera) + info_manager.loadCameraInfo() + + # Open video. + video = cv2.VideoCapture(args.video_file) + + # Get frame rate. + fps = video.get(cv2.cv.CV_CAP_PROP_FPS) + rate = rospy.Rate(fps) + + # Loop through video frames. + while not rospy.is_shutdown() and video.grab(): + tmp, img = video.retrieve() + + if not tmp: + print "Could not grab frame." + break + + img_out = np.empty((args.height, args.width, img.shape[2])) + + # Compute input/output aspect ratios. + aspect_ratio_in = np.float(img.shape[1]) / np.float(img.shape[0]) + aspect_ratio_out = np.float(args.width) / np.float(args.height) + + if aspect_ratio_in > aspect_ratio_out: + # Output is narrower than input -> crop left/right. + rsz_factor = np.float(args.height) / np.float(img.shape[0]) + img_rsz = cv2.resize(img, (0, 0), fx=rsz_factor, fy=rsz_factor, + interpolation=cv2.INTER_AREA) + + diff = (img_rsz.shape[1] - args.width) / 2 + img_out = img_rsz[:, diff:-diff-1, :] + elif aspect_ratio_in < aspect_ratio_out: + # Output is wider than input -> crop top/bottom. + rsz_factor = np.float(args.width) / np.float(img.shape[1]) + img_rsz = cv2.resize(img, (0, 0), fx=rsz_factor, fy=rsz_factor, + interpolation=cv2.INTER_AREA) + + diff = (img_rsz.shape[0] - args.height) / 2 + + img_out = img_rsz[diff:-diff-1, :, :] + else: + # Resize image. + img_out = cv2.resize(img, (args.height, args.width)) + + assert img_out.shape[0:2] == (args.height, args.width) + + try: + # Publish image. + img_msg = bridge.cv2_to_imgmsg(img_out, "bgr8") + img_msg.header.stamp = rospy.Time.now() + img_msg.header.frame_id = args.frame_id + img_pub.publish(img_msg) + + # Publish camera info. + info_msg = info_manager.getCameraInfo() + info_msg.header = img_msg.header + info_pub.publish(info_msg) + except CvBridgeError as err: + print err + + rate.sleep() + + return + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: +pass diff --git a/extract_msg/src/video_capture.py b/extract_msg/src/video_capture.py new file mode 100755 index 0000000..65f1cac --- /dev/null +++ b/extract_msg/src/video_capture.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python +from collections import deque +import numpy as np +#import imutils +import cv2 + +greenLower = (29, 86, 6) +greenUpper = (64, 255, 255) + +def main(): + #WindowName="live" + #cv2.namedWindow(WindowName) + cap = cv2.VideoCapture(1) + + if cap.isOpened(): + ret, frame = cap.read() + else: + ret = False + while ret: + + ret,frame = cap.read() + #output = cv2.cvtColor(frame,cv2.COLOR_BGR2RGB) + + #frame = imutils.resize(frame, width=600) + hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) + mask = cv2.inRange(hsv, greenLower, greenUpper) + mask = cv2.erode(mask, None, iterations=2) + mask = cv2.dilate(mask, None, iterations=2) + cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2] + center = None + + if len(cnts) > 0: + c = max(cnts, key=cv2.contourArea) + ((x, y), radius) = cv2.minEnclosingCircle(c) + if x < 300 : + print("move left "+ str(-1*(x-300)) +" spaces") + else : + print("move right " + str(-1*(300-x)) +" spaces") + + cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2) + + + + + #cv2.imshow(WindowName,output) + cv2.imshow("color original",frame) + cv2.imshow("Mask", mask) + if cv2.waitKey(1) == 27: + break + + cv2.destroyAllWindow() + + cap.release() + + + +if __name__ == "__main__": + main() diff --git a/gps_reading/gpsval.py b/gps_reading/gpsval.py new file mode 100755 index 0000000..e740ffd --- /dev/null +++ b/gps_reading/gpsval.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python +import rospy +import time +from geometry_msgs.msg import Point +import serial + +point = Point() + +arduino = serial.Serial('/dev/ttyUSB0',9600) + + +def talker(): + pub = rospy.Publisher('gpsval', Point, queue_size=100) + rospy.init_node('gps_sender', anonymous=True) + rate = rospy.Rate(100) + + # 10hz + while not rospy.is_shutdown(): + rawdata=[] + rawdata=str(arduino.readline()) + #rospy.loginfo(point) + if rawdata[0] == 'L': + lat=float(rawdata[4:14]) + lon=float(rawdata[18:29]) + point.x= lat + point.y= lon + point.z= 0 + print(lat) + print(lon) + pub.publish(point) + rate.sleep() + #rospy.spin() + +if __name__ == '__main__': + talker() + \ No newline at end of file diff --git a/gps_reading/gpsval_sub.py b/gps_reading/gpsval_sub.py new file mode 100755 index 0000000..93cab7a --- /dev/null +++ b/gps_reading/gpsval_sub.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python +import rospy +from geometry_msgs.msg import Point + +def callback(data): + lat=data.x + lon=data.y + rospy.loginfo("latitude"+lat) + rospy.loginfo("longitude"+lon) + +def listener(): + + + rospy.init_node('getGPSval', anonymous=True) + + rospy.Subscriber("gpsval", Point, callback) + + # spin() simply keeps python from exiting until this node is stopped + rospy.spin() + +if __name__ == '__main__': + listener() \ No newline at end of file