how to combine between openCV and ROS navigation

asked 2019-04-23 08:02:05 -0500

Redhwan gravatar image

updated 2019-04-23 08:09:58 -0500

I am trying to combine between OpenCV and ROS navigation but something wrong in my code.

I need to divide the control system into two parts:

controller1: during the system detect my face. this is works with me.

controller2: the robot go to the last position of human when the robot loses detect my face but this not work!!!

I guess the wrong in this line:

 if (pos1 and quat1) is not None:

the last position is a value of d but here, I assumed (x=0.2, y=0).

this full my code:

#!/usr/bin/env python
from __future__ import division

import math
import rospy
import numpy as np
from numpy import *
from sensor_msgs.msg import Image
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import cv2, cv_bridge

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
import time

global net
global red_fov
global pos1
global quat1
global h , d
pos1 = None
quat1 = None

face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_default.xml')

class GoToPose():

    def __init__(self):
        rospy.init_node('nav_test', anonymous=False)

        self.goal_sent = False
        self.bridge = cv_bridge.CvBridge()
        self.image_sub = rospy.Subscriber('/usb_cam/image_raw',Image, self.image_callback)
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel_mux/input/teleop',Twist, queue_size=1)
        self.twist = Twist()
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        rospy.loginfo("Wait for the action server to come up")
    def image_callback(self, msg):
        global h , d
        d =0
        image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
        frame = cv2.resize(image, (0,0), fx=0.5, fy=0.5, interpolation=cv2.INTER_NEAREST)
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        faces = face_cascade.detectMultiScale(gray, 1.3, 5)
        for (x, y, w, h) in faces:
            d =  4500/(h*100)
            d = 1
            cv2.rectangle(frame, (x,y), (x+w,y+h), (255,0,0), 2)

        cv2.imshow('face ', frame)            

        if d == True:
        else :

    def controller1(self):
        self.twist.linear.x = 0.2

    def controller2(self):

        count = 0
        if (pos1 and quat1) is not None:
            print 'a' 
            self.goto(pos1, quat1)
            count = count + 1
            if(count == 1):
    def goto(self, pos, quat):

        # Send a goal
        self.goal_sent = True
        goal = MoveBaseGoal()
#        goal.target_pose.header.frame_id = 'camera_link'
        goal.target_pose.header.frame_id = 'base_link'
        goal.target_pose.header.stamp =
        goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000),Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4']))

        # Start moving

        # Allow TurtleBot up to 60 seconds to complete task
        success = self.move_base.wait_for_result(rospy.Duration(60)) 

        state = self.move_base.get_state()
        result = False

        if success and state == GoalStatus.SUCCEEDED:
        # We made it!
            result = True

        self.goal_sent = False
        return result
        print pos

    def shutdown(self):
        if self.goal_sent:

def main():
    ctrl = GoToPose()
    # Customize the following values so they are appropriate for your location
    position = {'x': 0.2 , 'y' : 0 ...
edit retag flag offensive close merge delete