From b39bfdc3d8e78d5ef50727c0ad3751245d58b9d1 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 6 Oct 2022 15:06:50 +0200 Subject: [PATCH 1/3] rm old vel 2d handling in python library --- .../crazyflie_examples/cmd_vel_2d.py | 45 ------------------- crazyflie_py/crazyflie_py/crazyflie.py | 10 ----- 2 files changed, 55 deletions(-) delete mode 100644 crazyflie_examples/crazyflie_examples/cmd_vel_2d.py diff --git a/crazyflie_examples/crazyflie_examples/cmd_vel_2d.py b/crazyflie_examples/crazyflie_examples/cmd_vel_2d.py deleted file mode 100644 index 37b48b314..000000000 --- a/crazyflie_examples/crazyflie_examples/cmd_vel_2d.py +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/env python - -import numpy as np -from pathlib import Path -import time -from crazyflie_py import * - - -def executeCommand(timeHelper, cf, height, duration, rate=100,): - - - start_time = timeHelper.time() - while not timeHelper.isShutdown(): - t = timeHelper.time() - start_time - if t > duration: - break - - cf.cmdVelocity2D(0.0, 0.0, height, yawrate=0.2 ) - - timeHelper.sleepForRate(rate) - - -def main(): - swarm = Crazyswarm() - timeHelper = swarm.timeHelper - cf = swarm.allcfs.crazyflies[0] - - rate = 30.0 - duration = 3.0 - Z = 0.5 - - - cf.takeoff(targetHeight=Z, duration=Z+1.0) - timeHelper.sleep(Z+2.0) - - executeCommand(timeHelper, cf, Z, duration, rate) - - time.sleep(0.5) #Necessary for Cflib backend since this is not implemented yet - cf.notifySetpointsStop() - cf.land(targetHeight=0.03, duration=Z+1.0) - timeHelper.sleep(Z+2.0) - - -if __name__ == "__main__": - main() diff --git a/crazyflie_py/crazyflie_py/crazyflie.py b/crazyflie_py/crazyflie_py/crazyflie.py index b6bc9faa0..73f386872 100644 --- a/crazyflie_py/crazyflie_py/crazyflie.py +++ b/crazyflie_py/crazyflie_py/crazyflie.py @@ -179,9 +179,6 @@ def __init__(self, node, id, initialPosition): self.cmdPositionMsg.header.frame_id = "/world" - self.cmdVelocity2DPublisher = node.create_publisher(Twist, prefix + "/cmd_vel_2d", 1) - self.cmdVelocity2DMsg = Twist() - # self.cmdVelocityWorldPublisher = rospy.Publisher(prefix + "/cmd_velocity_world", VelocityWorld, queue_size=1) # self.cmdVelocityWorldMsg = VelocityWorld() # self.cmdVelocityWorldMsg.header.seq = 0 @@ -622,13 +619,6 @@ def cmdPosition(self, pos, yaw = 0.): self.cmdPositionMsg.yaw = yaw self.cmdPositionPublisher.publish(self.cmdPositionMsg) - def cmdVelocity2D(self, vx_body, vy_body, height, yawrate = 0.0): - self.cmdVelocity2DMsg.linear.x = vx_body - self.cmdVelocity2DMsg.linear.y = vy_body - self.cmdVelocity2DMsg.linear.z = height - self.cmdVelocity2DMsg.angular.z = yawrate - self.cmdVelocity2DPublisher.publish(self.cmdVelocity2DMsg) - # def setLEDColor(self, r, g, b): # """Sets the color of the LED ring deck. From 16949d069ed8c0e0135508181127f6ee42ee6f86 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 6 Oct 2022 15:12:14 +0200 Subject: [PATCH 2/3] add setup.py warning about changing things in the launch file --- docs2/tutorials.rst | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/docs2/tutorials.rst b/docs2/tutorials.rst index 560bc9fb3..cab52ed22 100644 --- a/docs2/tutorials.rst +++ b/docs2/tutorials.rst @@ -264,6 +264,8 @@ Once you are happy, you can save the map with 'Save Map' in the SLAM toolbox pan If not, you could tweak with the parameters of the `SLAM toolbox `_ to get a better result. +.. warning:: + Anything you change in the launch file, you'll need to reinstall with 'colcon build --symlink-install' in the ros2_ws folder Connecting with Nav2 Bringup ---------------------------- @@ -385,6 +387,9 @@ Also try it out by putting obstacles along the path of the crazyflie like in the As you noticed, the movement around the obstacles are pretty conservative. You can tune the values in /config/nav2_params.yaml, like the global or local planner's inflation_layer or the size of the robot. Please check out `NAV2's tuning documentation `_ for more explanation of these values. +.. warning:: + Anything you change in the launch, config and data file, you'll need to reinstall with 'colcon build --symlink-install' in the ros2_ws folder. This is due to `this unresolved issue in colcon_core `_. + .. note:: Final note. The SLAM performance and navigation performance of the Crazyflie with the multiranger is doable but not perfect. We absolutely encourage you to tweak and tune the parameters to get something better! (And if you do, please share :D) From c61b416acd65eeb85eeb9812d2ff76861febc704 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 6 Oct 2022 15:18:19 +0200 Subject: [PATCH 3/3] remove from setup.py --- crazyflie_examples/setup.py | 1 - 1 file changed, 1 deletion(-) diff --git a/crazyflie_examples/setup.py b/crazyflie_examples/setup.py index fe1fd0395..002408ff6 100644 --- a/crazyflie_examples/setup.py +++ b/crazyflie_examples/setup.py @@ -30,7 +30,6 @@ 'nice_hover = crazyflie_examples.nice_hover:main', 'figure8 = crazyflie_examples.figure8:main', 'cmd_full_state = crazyflie_examples.cmd_full_state:main', - 'cmd_vel_2d = crazyflie_examples.cmd_vel_2d:main', ], }, )