simondlevy/ardroneautopylot

Auto-Pilot the Parrot AR.Drone from Python (or Matlab or C)

ardroneautopylot's Issues

Failure when trying to integrate the code with ROS

Hi!

I am trying to publish camera frames into a ROStopic, so I modified autopylot_agent.py as you can see below:

import numpy as np
import cv2
import rospy
from sensor_msgs.msg import Image
import cv_bridge

rospy.init_node('dron_camera')
pub = rospy.Publisher('frame', Image,queue=10)
bridge = cv_bridge.CVBridge()

def action(img_bytes, img_width, img_height, is_belly, \
    ctrl_state, vbat_flying_percentage, theta, phi, psi, altitude, vx, vy):

    # Report navigation data
    print('ctrl state=%6d battery=%2d%% theta=%+f phi=%+f psi=%+f altitude=%+3d vx=%f vy=%+f' % \
                          (ctrl_state, vbat_flying_percentage, theta, phi, psi, altitude, vx, vy))

    # Create full-color image from bytes
    image_cv = np.frombuffer(img_bytes, np.uint8)
    image_cv = np.ndarray.reshape(image_cv, (img_height, img_width, 3))
    image_ros = bridge.cv2_to_msgimg(image_cv)
    pub.publish(image_ros)
    

    #cv2.imshow('frame', image_cv)
    #cv2.waitKey(3)

    # Set up commands for a clockwise turn
    zap = 0
    phi = 0 
    theta = 0 
    gaz = 0 
    yaw = 1

    return (zap, phi, theta, gaz, yaw)

And got this error:

dave@dave:~/catkin_ws/ARDroneAutoPylot$ ./ardrone_autopylot
Setting locale to en_GB.UTF-8
===================+> 192.168.1.1
Input device Sony PLAYSTATION(R)3 Controller found
Input device js2 added
Starting thread video_stage
Set IP_TOS ok
Starting thread navdata_update
Starting thread ardrone_control
PA : MEMORY SPACE ALLOWED : 40 MB
Start thread thread_academy_upload
Start thread thread_academy
Start thread thread_academy_download 
Academy download stage resumed
Thread navdata_update in progress...

video stage thread initialisation

Video multisocket : init 2 sockets
Video multisocket : connecting socket 0 on port 5555 UDP
Video multisocket : connecting socket 1 on port 5555 TCP
Traceback (most recent call last):
  File "/home/dave/catkin_ws/ARDroneAutoPylot/autopylot_agent.py", line 24, in <module>
    import rospy
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/__init__.py", line 49, in <module>
    from .client import spin, myargv, init_node, \
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/client.py", line 54, in <module>
    import rospy.core
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/core.py", line 71, in <module>
    from rospy.names import *
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/names.py", line 86, in <module>
    _mappings = load_mappings(sys.argv)
AttributeError: 'module' object has no attribute 'argv'

I tried to fix it adding PySys_SetArgv(argc, argv); in autopylot_python_agent.c just before Py_Initialize(); also I have also added int argc, char **argv as arguements of the fuction agent init

void agent_init(int argc, char **argv)
{
		
	PySys_SetArgv(argc, argv);

	Py_Initialize();
	
	PyObject * pName = PyString_FromString(AGENT_MODULE_NAME);
	
	pModule = PyImport_Import(pName);
	
	Py_DECREF(pName);

but that did not solve the problem:

dave@dave:~/catkin_ws/ARDroneAutoPylot$ ./ardrone_autopylot
Setting locale to en_GB.UTF-8
===================+> 192.168.1.1
Input device Sony PLAYSTATION(R)3 Controller found
Input device js2 added
Starting thread video_stage
Set IP_TOS ok
Starting thread navdata_update
Starting thread ardrone_control
PA : MEMORY SPACE ALLOWED : 40 MB
Start thread thread_academy_upload
Start thread thread_academy
Start thread thread_academy_download 
Academy download stage resumed
Thread navdata_update in progress...

video stage thread initialisation

Video multisocket : init 2 sockets
Video multisocket : connecting socket 0 on port 5555 UDP
Video multisocket : connecting socket 1 on port 5555 TCP
Segmentation fault (core dumped)

Could you help me with that? Have you tried this before? I do not know what that arguments (argc and argv) are. Any comment/idea is more than welcome.

Thanks in advance for your help,

David