Ce serveur Gitlab sera éteint le 30 juin 2020, pensez à migrer vos projets vers les serveurs gitlab-research.centralesupelec.fr et gitlab-student.centralesupelec.fr !

Commit 8093f402 authored by jad's avatar jad

before merge

parents f63967ba a9fc5f01
<<<<<<< HEAD
=======
>>>>>>> test
# Semi-Autonomous Drone Pilot
This ROS package is for a semi-autonomous drone pilot for the Parrot Bebop 2 using image processing (OpenCV) and unsupervised machine learning (Clustering).
## Quick Intro
This project has 2 parts, the first one is some nodes to ensure the navigation of the drone in constant velocities on the X,Y and Z axis since the Twist command provided by ROS is not a velocity but an acceleration.
The Second part is built on the first one and it some nodes doing image processing and sending necessary data to the other nodes in order to navigate the drone in the middle of A hallway.
## NOTE:
The First part is totaly isolated from the second one and can be used on any Parrot Bebop Drone.
In this File , you can see how to use the first one, the second one is coming soon
This project has many parts,The first one is moving the drone at a constant velocity.
The Second part is built on the first one , is used move the drone inside a hallway.
The third part, is used to detect openned doors.
## Moving The Drone
## Use This Package
First Clone this repo in your catkin workspace
```
$ git clone https://gitlab.centralesupelec.fr/obeid_jad/dorne_project.git drone_project
......@@ -20,40 +21,85 @@ Build your workspace
```
$ catkin build
```
To use this feature you need to launch the VelLaunch.launch
## Moving Drone
To do this you need to launch the VelLaunch.launch
```
$ roslaunch drone_project VelLaunch.launch
```
First start with sending a reset command to this modules by sending any integer to the topics ```/reset_cmd_x```,```/reset_cmd_y``` and ```/reset_cmd_z```.
And then publish to ```/vel_in_x``` , ``` /vel_in_y``` and ```/vel_in_z``` the velocities you want the drone to move with.
(Recommanded)
The frequency of sending data to the drone is equal to 5Hz.
Wait for receiving the acknoledge from the drone , on the topics ```/ack_res_x``` , ```/ack_res_y``` and ```/ack_res_z```. Once the reset is done you will receive ```1``` on these 3 topics.
### Example Moving at constant Velocity:
The ``` move_tester_test.py ``` file is an example of moving with a constant velocity.
And then publish to ```/vel_in_x``` , ``` /vel_in_y``` and ```/vel_in_z``` the velocities you want the drone to move with.
## Using The Interface
To run the other parts of the project, you need to launch the file ```Sequencer.launch```.
```
$ roslaunch drone_project Sequencer.launch
```
You will get an interface, that will let you choose the mode you want, you can click on Doors, to find opened doors and pass through them, or Hallway to launch the autonomous navigation in the Hallways.
## Some Useful Nodes
### Activation and Deactivation
Note that the rate of sending data to the drone is equal to the fastest rate between the 3 ``` /vel_in ``` topics.
To activate or deactivate a node, you need to send an activation to the topic ```/activations```, The activation command is a ```<nodeName>_1``` to activate and ```<nodeName>_0``` to deactivate.
### Example:
The ``` move_tester.py ``` file is an example of moving with a constant velocity.
The ``` turn_to_angle.py ``` file is an example of defining a specific angle and moving the drone to this angle.
Note that sending ```reset``` will deactivate all nodes.
## Indoor navigation
### Hallways navigation
Soon ...
### Distance Estimation using camera
This project contains a node able to estimate distances of moving objects using dense optical flow with the "Gunnar Farneback" algorithm.
nodeName = checkDoors
To use this module you can just run the ```testDenseOpticalFlow.py``` node :
```
$ rosrun drone_project testDenseOpticalFlow.py
```
and to view the histogram showing distances, run the rqt_image_view and choose the topic ```/image_graph```
And to view the histogram showing distances, run the rqt_image_view and choose the topic ```/image_graph```
```
$ rqt_image_view
```
Whenever an opened door is detected, this node will publish ```1```, to the topic ```/door_det ```.
### Vanishing point detection
This project contains a node able to detect the vanishing point in a Hallway using DBSCAN clustering algorithm.
nodeName = detectVanish
To use this you need to run the ```image_proc.py``` node :
```
$ rosrun drone_project image_proc.py
```
This node will publish the x of this point to the topic ```/centroids```.
### Turn to a specified angle
This project contains a node able to turn the drone to a specific angle.
nodeName = turnAng
To use this you need to run the ```turnAng.py``` node :
```
$ rosrun drone_project turnAng.py
```
If you run this node directly, the drone will move to 90 degrees.
If not, publish the desired angle in degrees to the topic ```/ang_in```.
<<<<<<< HEAD
This node will print out in the terminal whenever an opened door is detected and it will specify where is the door on the image
=======
>>>>>>> test
......@@ -2,19 +2,19 @@
<node pkg="drone_project" name="ComputeX" type="compute_x.py">
<remap from="/tar_in" to="/vel_in_x"/>
<remap from="/vel_out" to="/vel_out_x"/>
<remap from="/curr_data" to="/bebop/odom" />
<remap from="/curr_in" to="/bebop/odom" />
<remap from="/reset_cmd" to="/reset_cmd_x"/>
</node>
<node pkg="drone_project" name="ComputeY" type="compute_y.py">
<remap from="/tar_in" to="/vel_in_y"/>
<remap from="/vel_out" to="/vel_out_y"/>
<remap from="/curr_data" to="/bebop/odom" />
<remap from="/curr_in" to="/bebop/odom" />
<remap from="/reset_cmd" to="/reset_cmd_y"/>
</node>
<node pkg="drone_project" name="ComputeZ" type="compute_z.py">
<remap from="/tar_in" to="/vel_in_z"/>
<remap from="/vel_out" to="/vel_out_z"/>
<remap from="/curr_data" to="/bebop/odom" />
<remap from="/curr_in" to="/bebop/odom" />
<remap from="/reset_cmd" to="/reset_cmd_z"/>
</node>
<node pkg="drone_project" name="sendCommand" type="send_cmd.py">
......
<launch>
<node pkg="drone_project" name="Sequencer" type="Sequencer.py">
</node>
<node pkg="drone_project" name="Main" type="Main.py">
</node>
<node pkg="drone_project" name="optFlow" type="testDenseOpticalFlow.py">
</node>
<node pkg="drone_project" name="curveMotion" type="turn_to_angle.py">
</node>
<node pkg="drone_project" name="turnAng" type="turn_ang.py">
</node>
<node pkg="drone_project" name="resetCmd" type="reset_cmd_node.py">
</node>
</launch>
\ No newline at end of file
#!/usr/bin/env python
import rospy
from std_msgs.msg import Int32,Float32
from std_msgs.msg import Int32,Float32,String
import sys
from PyQt4 import QtGui,QtCore
......@@ -10,14 +10,7 @@ class Window(QtGui.QMainWindow):
super(Window,self).__init__()
self.setGeometry(50,50,500,300)
self.setWindowTitle("Command Test")
self.pub = rospy.Publisher("/mode", Int32, queue_size=1)
self.pub.publish(0)
self.pubx = rospy.Publisher("/in_vel_x", Float32, queue_size=1)
self.puby = rospy.Publisher("/in_vel_y", Float32, queue_size=1)
rospy.init_node("Project_Master", anonymous=True)
self.xval = 0.2
self.yval = 0
self.mode_pub = rospy.Publisher("/mode",String,queue_size=1)
self.home()
def home(self):
btn = QtGui.QPushButton("Quit",self)
......@@ -30,40 +23,37 @@ class Window(QtGui.QMainWindow):
btn.resize(100,70)
btn.move(10,10)
btn = QtGui.QPushButton("Constant V",self)
btn.clicked.connect(self.cnstnt_v)
btn = QtGui.QPushButton("Stairs",self)
btn.clicked.connect(self.Stairs)
btn.resize(100,70)
btn.move(120,10)
btn = QtGui.QPushButton("Freeze",self)
btn.clicked.connect(self.freeze_drone)
btn = QtGui.QPushButton("Doors",self)
btn.clicked.connect(self.Doors)
btn.resize(100,70)
btn.move(230,10)
btn = QtGui.QPushButton("Freeze",self)
btn.clicked.connect(self.Freeze)
btn.resize(100,70)
btn.move(340,10)
self.show()
def hallway_nav(self):
#In this Mode the drone will navigate in the hallways
self.pubx.publish(self.xval)
cmd = 1;
self.pub.publish(cmd)
def cnstnt_v(self):
#In this mode the drone will move at a givem velocity ,
#this velocity is predefined here in the code and it will be sent to /in_vel_x and /in_vel_y
self.pubx.publish(self.xval)
self.puby.publish(self.yval)
cmd = 2;
self.pub.publish(cmd)
def freeze_drone(self):
#In this mode the drone will receive a twist of zero and will stay stable in the position
cmd = 0;
self.pub.publish(cmd)
self.mode_pub.publish("hallway")
def Stairs(self):
self.mode_pub.publish("stairs")
def Doors(self):
self.mode_pub.publish("doors")
def Freeze(self):
self.mode_pub.publish("init")
def close_app(self):
print "Closing"
sys.exit()
def run():
rospy.init_node('Main', anonymous=True)
app = QtGui.QApplication(sys.argv)
GUI = Window()
sys.exit(app.exec_())
run()
\ No newline at end of file
......@@ -19,22 +19,20 @@ class RegulatorClass(object):
self.ack_reset = rospy.Publisher("/ack_res",Int32,queue_size=1)
self.currVal = 0
self.targVal = 0
self.reset_ack = 0
self.data_rec = 0
# self.activation = 0
def read_tar(self,ros_data):
print "tar received"
#if(self.activation == 0):
# return
self.targVal = ros_data.data
self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
self.cmd_publisher.publish(self.cmd)
self.data_rec = 1
#self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
#self.cmd_publisher.publish(self.cmd)
print "cmd sent"
#def check_activation(self,ros_data):
# print "Activation received"
# self.activation = ros_data.data
# if self.activation == 0:
# self.cmd_publisher.publish(0.0)
def reset_cmd(self,ros_data):
self.dc.cmd = 0
self.dc.TotErr = 0
self.dc.oldErr = 0
self.ack_reset.publish(1)
\ No newline at end of file
self.reset_ack = 1
\ No newline at end of file
No preview for this file type
#!/usr/bin/env python
"""
This node is the sequencer, This node will subscribe to the topic mode ,
and will choose the nodes to activate sequentially
"""
import rospy
from std_msgs.msg import String,Int32,Float32
from projectTools import Sequence
import sys
class Sequencer(object):
def __init__(self):
self.mode_sub = rospy.Subscriber("/mode",String,self.read_mode)
self.mode_pub = rospy.Publisher("/mode",String,queue_size=1)
self.actv_pub = rospy.Publisher("/activations",String,queue_size=1)
self.loop_pub = rospy.Publisher("/loop_seq",Int32,queue_size=1)
self.loop_sub = rospy.Subscriber("/loop_seq",Int32,self.enter_loop)
self.turn_done_sub = rospy.Subscriber("/turn_done",Int32,self.turn_done_callback)
self.det_doors_sub = rospy.Subscriber("/door_det",Int32,self.door_det_callback)
self.mode = "init"
self.doors_seq = Sequence("doors")
self.stairs_seq = Sequence("stairs")
self.hallway_seq = Sequence("hallway")
self.init_seq = Sequence("init")
self.rate = rospy.Rate(20) #10Hz
def turn_done_callback(self,ros_data):
self.doors_seq.set_phase(1)
self.doors_seq.set_published(False)
def door_det_callback(self,ros_data):
self.doors_seq.set_phase(2)
self.doors_seq.set_published(False)
def read_mode(self,ros_data):
self.mode = ros_data.data
self.loop_pub.publish(3)
def enter_loop(self,ros_data):
if(self.mode == "init"):
self.doors_seq.reset_seq()
self.reset_seq_func()
pass
if(self.mode == self.doors_seq.get_mode()):
self.doors_seq_func()
pass
if(self.mode == self.stairs_seq.get_mode()):
self.stairs_seq_func()
pass
if(self.mode == self.hallway_seq.get_mode()):
self.hallway_seq_func()
pass
def reset_seq_func(self):
self.actv_pub.publish("reset")
self.actv_pub.publish("resetCmd_1")
def stairs_seq_func(self):
pass
def hallway_seq_func(self):
pass
def doors_seq_func(self):
if(self.doors_seq.get_phase() == 0 ):
if (self.doors_seq.get_publ() == False) :
self.actv_pub.publish("reset")
self.rate.sleep()
self.actv_pub.publish("curveMotion_1")
self.rate.sleep()
self.doors_seq.set_published(True)
if(self.mode != "init"):
self.loop_pub.publish(1)
self.rate.sleep()
if(self.doors_seq.get_phase() == 1):
if(self.doors_seq.get_publ() == False):
self.actv_pub.publish("reset")
self.rate.sleep()
self.actv_pub.publish("checkDoors_1")
self.actv_pub.publish("turnAng_1")
self.rate.sleep()
self.doors_seq.set_published(True)
if(self.mode != "init"):
self.loop_pub.publish(2)
self.rate.sleep()
if(self.doors_seq.get_phase() == 2):
self.actv_pub.publish("reset")
self.rate.sleep()
self.mode_pub.publish('init')
def main(args):
rospy.init_node('Sequencer', anonymous=True)
sc = Sequencer()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
\ No newline at end of file
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
class NodeActivate(object):
def __init__(self,my_name):
self._my_name = my_name
self.act_sub = rospy.Subscriber("/activations",String,self.check_act)
self.node_active = 0
def check_act(self,ros_data):
msg = ros_data.data
if(msg == 'reset'):
self.node_active = 0
else:
msg,act = msg.split("_")
if(msg == self._my_name):
self.node_active = act
\ No newline at end of file
#!/usr/bin/env python
"""
This node will publish the desired velocity along the x axis on the topic /vel_in_x
"""
import numpy as np
import rospy
import sys
from projectTools import GenTools,DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
from nav_msgs.msg import Odometry
from activation_class import NodeActivate
class ComputeXTar(NodeActivate):
def __init__(self):
super(ComputeXTar,self).__init__("compute_tar_x")
cmd_reset = rospy.Publisher("/reset_cmd_x",Int32,queue_size=1)
self.sync_sub = rospy.Subscriber("/bebop/odom",Odometry,self.sync_cmd)
self.cmd_publisher = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
self.targX = 0.3
def sync_cmd(self,ros_data):
if(self.node_active == 0):
return
self.cmd_publisher.publish(self.targX)
def main(args):
rospy.init_node('computeTarX', anonymous=True)
sc = ComputeXTar()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
......@@ -10,15 +10,20 @@ import sys
from projectTools import GenTools,DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
from activation_class import NodeActivate
class ComputeYTar:
class ComputeYTar(NodeActivate):
def __init__(self):
super(ComputeYTar,self).__init__("compute_tar_y")
cmd_reset = rospy.Publisher("/reset_cmd_y",Int32,queue_size=1)
self.sDiff_subscriber = rospy.Subscriber("/sDiffs",Float32,self.read_sDiffs)
self.cmd_publisher = rospy.Publisher("/vel_in_y",Float32,queue_size=1)
self.act_publisher = rospy.Publisher("/activation_y",Int32,queue_size=1)
self.targY = 0
#self.act_publisher.publish(0)
def read_sDiffs(self,ros_data):
if(self.node_active == 0):
return
#read the centroid and send the data
self.targY = ros_data.data
#The value of the velocity is proportional to the value of the difference of the values
......
......@@ -10,9 +10,12 @@ import sys
from projectTools import GenTools,DroneCommand
from Regulator import RegulatorClass
from std_msgs.msg import Float32,Int32
from activation_class import NodeActivate
class ComputeZTar:
class ComputeZTar(NodeActivate):
def __init__(self):
super(ComputeZTar,self).__init__("compute_tar_z")
cmd_reset = rospy.Publisher("/reset_cmd_z",Int32,queue_size=1)
self.dc = DroneCommand(0.007,0,0)
self.centroid_subscriber = rospy.Subscriber("/centroids",Float32,self.read_centroid)
self.cmd_publisher = rospy.Publisher("/vel_in_z",Float32,queue_size=1)
......@@ -23,6 +26,8 @@ class ComputeZTar:
self.act_publisher.publish(1)
def read_centroid(self,ros_data):
#read the centroid and send the data
if(self.node_active == 0):
return
self.centroid = ros_data.data
self.cmd = self.dc.computeCommand(self.centroid,self.TargCentroid)
#Set the max value to 0.3 in absolute value
......
......@@ -13,9 +13,16 @@ from std_msgs.msg import Float32
class XCommand(RegulatorClass):
def __init__(self):
super(XCommand,self).__init__(0.7,0.03,0.6)
#super(XCommand,self).__init__(0.8,0.0,0.7)
self.deb = rospy.Publisher("/deb_x",Float32,queue_size=1)
def read_val(self,ros_data):
twist = ros_data.twist.twist
self.currVal = twist.linear.x
self.deb.publish(0)
self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
self.data_rec = 0
print "Val Read"
def main(args):
rospy.init_node('computeX', anonymous=True)
......
......@@ -15,7 +15,7 @@ from Regulator import RegulatorClass
class YCommand(RegulatorClass):
def __init__(self):
super(YCommand,self).__init__(0.7,0.03,0.6)
super(YCommand,self).__init__(0.4,0.03,0.8)
def read_val(self,ros_data):
#In this Topic here we need to send the command to the drone
......@@ -27,6 +27,10 @@ class YCommand(RegulatorClass):
self.targVal = GenTools.setMax(self.targVal,0.3)
#ycmd = self.dc.computeCommand(self.currVal,self.targVal)
#self.cmd_publisher.publish(ycmd)
self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
self.data_rec = 0
def main(args):
rospy.init_node('computeY', anonymous=True)
......
......@@ -17,9 +17,14 @@ class ZCommand(RegulatorClass):
def read_tar(self,ros_data):
#if(self.activation == 0):
# return
self.cmd_publisher.publish(ros_data.data)
#self.cmd_publisher.publish(ros_data.data)
self.cmd = ros_data.data
self.data_rec = 1
def read_val(self,ros_data):
pass
#self.cmd = self.dc.computeCommand(self.currVal,self.targVal)
if(self.data_rec == 1):
self.cmd_publisher.publish(self.cmd)
self.data_rec = 0
def main(args):
rospy.init_node('computeZ', anonymous=True)
......
import rospy
from std_msgs.msg import Float32,Int32
......@@ -20,9 +20,11 @@ import math
from cv_bridge import CvBridge, CvBridgeError
from itertools import combinations, chain
import datetime
from activation_class import NodeActivate
class image_convert:
class image_convert(NodeActivate):
def __init__(self):
super(image_convert,self).__init__("detectVanish")
self.pub = rospy.Publisher("/centroids", Float32, queue_size=1)
self.pubDiff = rospy.Publisher('/sDiffs',Float32,queue_size=1)
self.pubForBag = rospy.Publisher('/imgForBag/front/compressed', CompressedImage, queue_size=1)
......@@ -194,6 +196,8 @@ class image_convert:
def callback(self,ros_data):
#msg = ""
if(self.node_active == 0):
return
cv2_img = self.br.imgmsg_to_cv2(ros_data)
#np_arr = np.fromstring(ros_data.data, np.uint8)
#image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
......@@ -211,7 +215,7 @@ def main(args):
rospy.init_node('image_feature', anonymous=True)
ic = image_convert()
rospy.spin()
ic.saveData()
#ic.saveData()
cv2.destroyAllWindows()
......
......@@ -58,6 +58,7 @@ class TurnDrone:
rate.sleep()
def main(args):
rospy.init_node('MoveTest', anonymous=True)
sc = TurnDrone()
#rospy.init_node('send_command', anonymous=True)
if __name__ == '__main__':
......
......@@ -23,9 +23,9 @@ def talker():
rate.sleep()
start = time.time()
end = time.time()
while end - start < 1:
while end - start < 8:
end = time.time()
velx = 0.2
velx = 0.3
vely = 0.0
velz = 0.0
......@@ -36,17 +36,6 @@ def talker():
rate.sleep()
start = time.time()
end = time.time()
while end - start < 4:
end = time.time()
velx = 0.0
vely = 0.0
velz = -0.4
pubvel1.publish(velx)
pubvel2.publish(vely)
pubvel3.publish(velz)
rate.sleep()
if __name__ == '__main__':
try:
talker()
......
......@@ -18,6 +18,24 @@ class DroneCommand:
Derr = 0
self.cmd = self._P*err + self.TotErr*self._I + self._D*Derr
return self.cmd
class Sequence:
def __init__(self,mode='init'):
self._mode = mode
self._phase = 0
self._published=False
def get_phase(self):
return self._phase
def get_mode(self):
return self._mode
def get_publ(self):
return self._published
def set_phase(self,val):
self._phase = val
def set_published(self,val):
self._published = val
def reset_seq(self):
self._published = False
self._phase = 0
class GenTools:
def __init__(self):
pass
......
No preview for this file type
#!/usr/bin/env python
import rospy
import sys
from std_msgs.msg import Float32,Int32
from activation_class import NodeActivate
from nav_msgs.msg import Odometry
class resetCmd(NodeActivate):
def __init__(self):
super(resetCmd,self).__init__("resetCmd")
self.pubres1 = rospy.Publisher("/reset_cmd_x", Int32, queue_size=1)
self.pubres2 = rospy.Publisher("/reset_cmd_y", Int32, queue_size=1)
self.pubres3 = rospy.Publisher("/reset_cmd_z", Int32, queue_size=1)
self.sync_sub = rospy.Subscriber("/bebop/odom",Odometry,self.reset_cmd_func)
self.rate = rospy.Rate(10)
def reset_cmd_func(self,ros_data):
if(self.node_active == 0):
return
for i in range(5) :
self.pubres1.publish(1)
self.pubres2.publish(1)
self.pubres3.publish(1)
self.rate.sleep()
self.node_active = 0
def main(args):
rospy.init_node('resetCmd', anonymous=True)
sc = resetCmd()
#rospy.init_node('send_command', anonymous=True)
rospy.spin()
if __name__ == '__main__':
main(sys.argv)
\ No newline at end of file
......@@ -8,17 +8,17 @@ import cv2
import rospy
from sensor_msgs.msg import CompressedImage
from sensor_msgs.msg import Image
from std_msgs.msg import Float32
from std_msgs.msg import Float32,Int32
import sys
import math
from cv_bridge import CvBridge, CvBridgeError
import random
from nav_msgs.msg import Odometry
from activation_class import NodeActivate
class EnterDoors:
class EnterDoors(NodeActivate):
def __init__(self):
self.img_read = rospy.Subscriber("/bebop/image_raw/compressed",CompressedImage,self.transform_image)
self.img_pub = rospy.Publisher("/image_graph",Image,queue_size=1)
self.roi_pub = rospy.Publisher("/image_roi",Image,queue_size=1)
super(EnterDoors,self).__init__("checkDoors")
self.br = CvBridge()
self.prvs = None
self.next = None
......@@ -26,11 +26,40 @@ class EnterDoors:
self.plot_image = None
self.old_dat = None
self.alpha = 0.25
self.alpha2 = 0.85
self.thresh = 20
self.alpha2 = 0.35
self.thresh = 200
self.dat_bin = None
# Parameters for lucas kanade optical flow
self.my_vel = 0
self.conf = 0
self.img_read = rospy.Subscriber("/bebop/image_raw/compressed",CompressedImage,self.transform_image)
self.img_pub = rospy.Publisher("/image_graph",Image,queue_size=1)
self.roi_pub = rospy.Publisher("/image_roi",Image,queue_size=1)
self.vel_sub = rospy.Subscriber("/bebop/odom",Odometry,self.update_vel)
self.vel_pub_x = rospy.Publisher("/vel_in_x",Float32,queue_size=1)
self.vel_pub_y = rospy.Publisher("/vel_in_y",Float32,queue_size=1)
self.door_pub = rospy.Publisher("/door_det",Int32,queue_size=1)
self.y_vel = 0
def update_vel(self,ros_data):
if(self.node_active == 0):
return
twist = ros_data.twist.twist
self.my_vel = np.absolute(twist.linear.y)
curr_pose = ros_data.pose.pose
cur_ang_quat = curr_pose.orientation.z
if(cur_ang_quat > 0):
self.y_vel = -0.3
else:
self.y_vel = 0.3
def transform_image(self,ros_data):
if(self.node_active == 0):
return
self.vel_pub_x.publish(0.0)
self.vel_pub_y.publish(self.y_vel)