About where to start obstacle avoidance demo

[lwisle]

Hello！I want to make a obstacle avoidance demo, but now I don't know where should I start.

My initial idea is:
Firstly, create obstacle rigid bodies in optitrack and send position information to crazyswarm.
Then, send goTo command to crazyflie.
Finally, when crazyflie executes goTo command, in the crazyswarm the python script calculates whether crazyflie is too close to the obstacle. If so, it will land immediately.

The question are:
1.where should I modify to make a ROS node which Include obstacle location information to be subscribed？
2.How can I do the distance calculation at the same time I use goTo command?
Could you please give me some advice？Thanks.

[lwisle]

Now I use vrpn_client_ros to get obstacle's position information, then subscribe it in python file.
But I still don't know how to do the distance calculation at the same time I use goTo command......
here is my code in scripts(Need to run vrpn firstly):

from pycrazyswarm import Crazyswarm
import numpy as np
import rospy
from geometry_msgs.msg import PoseStamped

TAKEOFF_DURATION = 2.5
HOVER_DURATION = 5.0

def posCallback(msg):
    print(msg)


def main():
    swarm = Crazyswarm()
    timeHelper = swarm.timeHelper
    cf = swarm.allcfs.crazyflies[0]
    
    #rospy.init_node('pose_subscriber',anonymous=True)
    #rospy.Subscriber("/vrpn_client_node/obstacle/pose",PoseStamped,posCallback)
    data = rospy.wait_for_message("/vrpn_client_node/obstacle/pose",PoseStamped,timeout=None)
    print(data)
    
    cf.takeoff(targetHeight=0.5, duration=TAKEOFF_DURATION)
    timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION)
    #pos = np.array(cf.initialPosition)
    cf.goTo([1.0, 1.0, 1.5],0,10)
    timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION)
    cf.land(targetHeight=0.04, duration=2.5)
    timeHelper.sleep(TAKEOFF_DURATION)
    #rospy.spin()

if __name__ == "__main__":
    main()

[jpreiss]

I still don't know how to do the distance calculation at the same time I use goTo command...

I am not sure what exactly you mean here, but the goTo command returns immediately. Many of our examples call sleep immediately after calling goTo, but it's not required. You can do other work while you wait for the CF to execute the goTo command.

goTo is also interruptible. If you call land before the goTo is finished, the CF will land immediately.

[lwisle]

Thank you，I probably know what to do.