Missing blades after using my own controller

[Yiqing-Gu]

Hi,

I am trying to use my own controller to control crazyflies in Gazebo. However, the blades of crazyflies will disappear and crazyflies will stay on the ground after using my own controller.

My controller codes:

        import numpy as np
        import rospy
        from mav_msgs.msg import DroneState

        class TBController:
        def __init__(self):
            rospy.init_node('tb_cntrl', anonymous=True)

            self.cmd_vel_pub = rospy.Publisher('drone_state', DroneState, queue_size=10)
            self.rate = rospy.Rate(100)

        def start(self):
            state = DroneState()
            while not rospy.is_shutdown():
                # Controller code goes between these comments
                # Differential drive robot with forward speed and angular rate

                t = rospy.get_time()

                state.position.x = np.cos(2*np.pi*t)

                state.position.y = np.sin(2*np.pi*t)

                state.position.z = 5

                self.cmd_vel_pub.publish(state)

                # END Controller code

                self.rate.sleep()


    if __name__ == '__main__':
        cntrl = TBController()
        cntrl.start()

In terms of my launch file, I used "crazyflie2_swarm_hovering_example" as my launch file but I deleted other crazyflies and kept only one left.

In order to use my own controller, I only changed

<node name="hovering_example_spline_swarm" pkg="rotors_gazebo" type="hovering_example_spline_swarm" output="screen" >
<rosparam command="load" file="$(find rotors_gazebo)/resource/sample_controller.py" />

by using

</node-->
<node name="customized_node" pkg="rotors_gazebo" type="sample_controller.py" />

"sample_controller" is the name of my controller file.

The given launch files worked successfully on my deceive. But I do not know what happened with my own controller.

Looking for any useful suggestions.

Thanks!

[gsilano]

Ok! Just to understand. Are you able to run the examples mentioned in the README.md file?

[Yiqing-Gu]

Ok! Just to understand. Are you able to run the examples mentioned in the README.md file?

Yes. Examples work perfectly for me.

[gsilano]

Okay, so I think you missed things. There is simply a problem with the xacro file used to load the drone and create the links between the components. Start from the crazyflie2_without_controller.launch lunch file to add your controller. There, you just need to add your Python script, and if the topics are right, you should see the drone take off.

[Yiqing-Gu]

Thanks for your advice.

However, My controller does not work in crazyflie2_without_controller.launch.

I want a crazyflie to move in circle in the simulation. The following is a part of codes in crazyflie2_without_controller.launch. I wonder whether is something wrong with my way of importing my controller?

<group ns="$(arg mav_name)">
<include file="$(find rotors_gazebo)/launch/spawn_mav_crazyflie.launch">
  <arg name="mav_name" value="$(arg mav_name)" />
  <arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
  <arg name="enable_logging" value="$(arg enable_logging)" />
  <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
  <arg name="enable_state_estimator" value="$(arg enable_state_estimator)" />
  <arg name="log_file" value="$(arg log_file)"/>
</include>

# I add my controller here
<node name="sample_controller" pkg="rotors_gazebo" type="sample_controller.py" />

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
</group>

Okay, so I think you missed things. There is simply a problem with the xacro file used to load the drone and create the links between the components. Start from the crazyflie2_without_controller.launch lunch file to add your controller. There, you just need to add your Python script, and if the topics are right, you should see the drone take off.

[gsilano]

Ok! So, the problem shouldn't be the launch file. In my experience, this error (the propeller suddenly disconnects from the drone's body as they fall to the floor) occurs when the controller doesn't publish the right content to drive the propellers. This could be related to the data format (i.e. the structure of the ROS message), the values ​​(weird, but it can happen) or the topic name. Could you tell me the name of the topic and the values ​​your Python script is pubblishing?

Maybe you are not importing the controller correctly. I mean, there's a problem with the name path. I am not very familiar with Python scripts. So, could you check that the format is similar to the one used in the sample file present in the repository? if you check well, there is a scripts folder in "rotors_gazebo", inside there are some Python scripts that are used in the code. Try searching for the corresponding launch file using the GitHub search bar. If you have troubles in it, I will give you back with more precise instructions.

[Yiqing-Gu]

Ok! So, the problem shouldn't be the launch file. In my experience, this error (the propeller suddenly disconnects from the drone's body as they fall to the floor) occurs when the controller doesn't publish the right content to drive the propellers. This could be related to the data format (i.e. the structure of the ROS message), the values ​​(weird, but it can happen) or the topic name. Could you tell me the name of the topic and the values ​​your Python script is pubblishing?

Maybe you are not importing the controller correctly. I mean, there's a problem with the name path. I am not very familiar with Python scripts. So, could you check that the format is similar to the one used in the sample file present in the repository? if you check well, there is a scripts folder in "rotors_gazebo", inside there are some Python scripts that are used in the code. Try searching for the corresponding launch file using the GitHub search bar. If you have troubles in it, I will give you back with more precise instructions.

So my controller will create a node called "sample_controller" and publish to the topic "DroneState". There are some parameters for x, y and z position in the DroneState. Like my start() function in codes for controller. I tried to make them keep updating and make a crazyflie move in simulation.

At the first time, I wrote my controller to replace the existing controller called hovering_example_spline_swarm in crazyflie2_swarm_hovering_example. I checked node graph in rqt and I found I should control crazyflie if I use my customized controller to replace it.

Are scripts in scripts folder about controller? I am not quite sure about it. Some existing controllers were implemented in C.

[gsilano]

OK! We got the issue. You can't publish there. You need to publish the propeller angular velocities (a mav_msgs::Actuators ROS message) on the mav_msgs::default_topics::COMMAND_ACTUATORS topic. You can find the name of the topic by looking at this link: https://github.com/gsilano/mav_comm/blob/master/mav_msgs/include/mav_msgs/default_topics.h.

Have you read the papers that are linked to the repository? This is the link to the Publications: https://github.com/gsilano/CrazyS/wiki/Publications

GitHub

mav_comm/default_topics.h at master · gsilano/mav_comm

This repository contains message and service definitions used for mavs. All future message definitions go in here, existing ones in other stacks should be moved here where possible. - mav_comm/defa...

GitHub

Publications · gsilano/CrazyS Wiki

CrazyS is an extension of the ROS package RotorS, aimed to modeling, developing and integrating the Crazyflie 2.0 - Publications · gsilano/CrazyS Wiki

[Yiqing-Gu]

OK! We got the issue. You can't publish there. You need to publish the propeller angular velocities (a mav_msgs::Actuators ROS message) on the mav_msgs::default_topics::COMMAND_ACTUATORS topic. You can find the name of the topic by looking at this link: https://github.com/gsilano/mav_comm/blob/master/mav_msgs/include/mav_msgs/default_topics.h.

Have you read the papers that are linked to the repository? This is the link to the Publications: https://github.com/gsilano/CrazyS/wiki/Publications

GitHub**mav_comm/default_topics.h at master · gsilano/mav_comm**This repository contains message and service definitions used for mavs. All future message definitions go in here, existing ones in other stacks should be moved here where possible. - mav_comm/defa...

GitHub**Publications · gsilano/CrazyS Wiki**CrazyS is an extension of the ROS package RotorS, aimed to modeling, developing and integrating the Crazyflie 2.0 - Publications · gsilano/CrazyS Wiki

Thanks for your help. I read the paper and checked links you gave me. But it seems that I did not explain what I am doing clearly.
I would like to develop a trajectory generator to control crazyflie. It is a high-level controller and it will generate the position of crazyfile. In my imagination, I can replace existing trajectory generator with my own generator and crazyflie will move by following my desired trajectories.
In terms of other parameters, I prefer to use what I have in the original repository. If I comprehend your suggestions correctly, you are talking about develop a lower-level controller.
So I am curious whether it is possible to develop a high-level controller, in other words, a trajectory generator here without revising corresponding low-level controllers?

GitHub

mav_comm/default_topics.h at master · gsilano/mav_comm

This repository contains message and service definitions used for mavs. All future message definitions go in here, existing ones in other stacks should be moved here where possible. - mav_comm/defa...

GitHub

Publications · gsilano/CrazyS Wiki

CrazyS is an extension of the ROS package RotorS, aimed to modeling, developing and integrating the Crazyflie 2.0 - Publications · gsilano/CrazyS Wiki

[gsilano]

Well, you can't do that because the current controller doesn't implement a trajectory tracking example. A PR (#86) was opened a while ago to add this functionality, but I haven't had time to finish the integration yet. So in case you want to take this idea further, you should also implement a trajectory tracking controller for Crazyflie.

[Yiqing-Gu]

My current plan is writing a trajectory generator and publish to "crazyflie2/command/trajectory". However, I am confused with how "crazyflie2/command/trajectory" works. I used "rostopic echo" to monitor "crazyflie2/command/trajectory". "crazyflie2/command/trajectory" will publish to "position_controller" only once.

If I use "hovering_example" as an example, the sequence of publishing should be "hovering_example" -> "crazyflie2/command/trajectory" -> "position_controller". I checked "hovering_example.cpp" and I have no clue about how coordinates got imported and updated. My guess it should be related to the following codes:

nh_private.param("x", desired_position.x(), desired_position.x());
nh_private.param("y", desired_position.y(), desired_position.y());
nh_private.param("z", desired_position.z(), desired_position.z());
nh_private.param("yaw", desired_yaw, desired_yaw);

Could you help me understand where the trajectory file is and how the data is transferred to "position_controller"?

Besides, I reinstalled my Ubuntu and reinstalled CrazyS. I always got some error while booting simulations. The following in error information:

ERROR: cannot launch node of type [robot_state_publisher/robot_state_publisher]: robot_state_publisher
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/cas-lab/catkin_ws/src/mav_comm/mav_comm
ROS path [2]=/home/cas-lab/catkin_ws/src/mav_comm/mav_msgs
ROS path [3]=/home/cas-lab/catkin_ws/src/mav_comm/mav_planning_msgs
ROS path [4]=/home/cas-lab/catkin_ws/src/mav_comm/mav_state_machine_msgs
ROS path [5]=/home/cas-lab/catkin_ws/src/mav_comm/mav_system_msgs
ROS path [6]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_comm
ROS path [7]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_control
ROS path [8]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_description
ROS path [9]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_evaluation
ROS path [10]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_gazebo_plugins
ROS path [11]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_gazebo
ROS path [12]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_joy_interface
ROS path [13]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_simulator
ROS path [14]=/home/cas-lab/catkin_ws/src/CrazyS/rqt_rotors
ROS path [15]=/opt/ros/noetic/share
ERROR: cannot launch node of type [joint_state_publisher/joint_state_publisher]: joint_state_publisher
ROS path [0]=/opt/ros/noetic/share/ros
ROS path [1]=/home/cas-lab/catkin_ws/src/mav_comm/mav_comm
ROS path [2]=/home/cas-lab/catkin_ws/src/mav_comm/mav_msgs
ROS path [3]=/home/cas-lab/catkin_ws/src/mav_comm/mav_planning_msgs
ROS path [4]=/home/cas-lab/catkin_ws/src/mav_comm/mav_state_machine_msgs
ROS path [5]=/home/cas-lab/catkin_ws/src/mav_comm/mav_system_msgs
ROS path [6]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_comm
ROS path [7]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_control
ROS path [8]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_description
ROS path [9]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_evaluation
ROS path [10]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_gazebo_plugins
ROS path [11]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_gazebo
ROS path [12]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_joy_interface
ROS path [13]=/home/cas-lab/catkin_ws/src/CrazyS/rotors_simulator
ROS path [14]=/home/cas-lab/catkin_ws/src/CrazyS/rqt_rotors
ROS path [15]=/opt/ros/noetic/share

However, my simulation still works for me. I followed installation in readme and sourced corresponding path. Whether there is something wrong with my configuration?

[gsilano]

Let me answer to your questions in order:

• As I said, the current position controller doesn't allow you to track a trajectory. Due to this, it is necessary to embed your own trajectory tracking controller. The controller should output the angular velocities of the prop used as an input to control the behavior of the quad-rotor.

• The position_controller_node.cpp file contains the name of the topics in which the reference trajectory is published. This part of the code contains the topics used to pass the reference path (single input or continuous signal, see the code) to the controller.

CrazyS/rotors_control/src/nodes/position_controller_node.cpp

Lines 58 to 79 in 4e1537a

	// Topics subscribe
	if (!enable_mellinger_controller_ && !enable_internal_model_controller_){
	cmd_multi_dof_joint_trajectory_sub_ = nh.subscribe(mav_msgs::default_topics::COMMAND_TRAJECTORY, 1,
	&PositionControllerNode::MultiDofJointTrajectoryCallback, this);
	odometry_sub_ = nh.subscribe(mav_msgs::default_topics::ODOMETRY, 1,
	&PositionControllerNode::OdometryCallback, this);
	}
	else{
	imu_ideal_sub_ = nh.subscribe(mav_msgs::default_topics::IMU, 1,
	&PositionControllerNode::IMUMellingerCallback, this);
	cmd_multi_dof_joint_trajectory_spline_sub_ = nh.subscribe(mav_msgs::default_topics::DRONE_STATE, 1,
	&PositionControllerNode::MultiDofJointTrajectoryMellingerCallback, this);
	odometry_sub_ = nh.subscribe(mav_msgs::default_topics::ODOMETRY, 1,
	&PositionControllerNode::MellingerOdometryCallback, this);
	}

• The ROS path error is due to the bashrc file. Be sure you are sourcing the CrazyS package correctly. Also, remember that you can only get one ROS Work Package per time, otherwise you have to extend the path. Here is a little guideline on this: https://www.theconstructsim.com/ros-5-mins-037-ros-package-path/

The Construct

[ROS in 5 mins] 037 - What is ROS PACKAGE PATH and how to use it? - The Construct

Hello ROS Developers Welcome to this new post about ROS. In today’s post, we are going to learn what is the ROS_PACKAGE_PATH variable and how it is used by the ROS ecosystem. The commands used here can be executed in your own computer if you have ROS installed, but for simplicity, we are going to use […]

[Yiqing-Gu]

Very appreciate for your help and advices.

I realized that I misunderstood how CrazyS works. Apologize for any inconvenience I brought to you.

I am currently in a research about trajectory of crazy flies. My plan is developing a trajectory tracking controller now. Could you give me some clues about which topics I need to work with? For example, since I need to output the angular velocities of the prop, which topic I need to publish?

[gsilano]

Well, you can try to port in CrazyS this nice controller: https://github.com/bcbarbara/crazyflie_nmpc. This is a contribution from a friend of mine, but so far I have had time to integrate it. You could help out with this. It would be greatly appreciated.

You need to publish on this topic: mav_msgs::default_topics::COMMAND_ACTUATORS. Here you find the mav_comm repository with the name definition of the topic.

I hope this helps

GitHub

GitHub - bcbarbara/crazyflie_nmpc: ROS stack with an efficient real-time NMPC for the Crazyflie 2.1

ROS stack with an efficient real-time NMPC for the Crazyflie 2.1 - GitHub - bcbarbara/crazyflie_nmpc: ROS stack with an efficient real-time NMPC for the Crazyflie 2.1