/ - Diff - Quadrotor - Robotics Club

Revision 7a6ed02d

ID	7a6ed02d47f49de02afc7e0e8cc97db92054f38c

Added by Thomas Mullins about 12 years ago

Major changes in mikrokopter for lost connection

-In nav_lib.cpp, added main_loop, so we don't have to keep rewriting it
:). Also, this now publishes to /mk_wrapper/control, which should be
used from now on. However, old publish_on still works as it did.

-Changed turn_to_target and joystick_control (but not any others yet) to
use new nav_lib main_loop.

-Added mk_wrapper, which simply copies messages from /mk_wrapper/control
to /mikrokopter/req_set_control. After recieving the first message, if
it receives no message for a period of 200ms (rosparam max_quiet_time),
it will instead publish 0's and a thrust of 60, ignoring all future
input until restarted.

-Added launch files onboard.launch and joystick.launch, so that we
remember what should run where and can easily set some parameters.

mikrokopter/CMakeLists.txt
30	30	target_link_libraries(keyboard_control ncurses)
31	31	rosbuild_add_executable(joystick_control src/joystick_control.cpp src/nav_lib.cpp)
32	32	rosbuild_add_executable(turn_to_target src/turn_to_target.cpp src/nav_lib.cpp)
	33	rosbuild_add_executable(mk_wrapper src/mk_wrapper.cpp)

     <launch>
       <node name="joy" pkg="joy" type="joy">
         <param name="/dev/ttyACM0" />
       </node>
       <node name="joystick_control" pkg="mikrokopter" type="joystick_control">
         <!-- rate in Hz to publish Control messages -->
         <param name="publish_rate" value="20" />
       </node>
     </launch>

mikrokopter/launch/onboard.launch
	1	<launch>
	2	<include file="$(find mikrokopter)/launch/mikrokopter.launch" />
	3	<node name="mk_wrapper" pkg="mikrokopter" type="mk_wrapper">
	4	<!-- if no Control message is received in this time in ms, go dead-->
	5	<param name="max_quiet_time" value="200" />
	6	</node>
	7	</launch>

mikrokopter/src/joystick_control.cpp
27	27
28	28	JoystickControl::JoystickControl()
29	29	{
30		pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
31	30	sub = n.subscribe<sensor_msgs::Joy>("joy", 100,
32	31	&JoystickControl::joyCallback, this);
33	32	}
34	33
35	34	void JoystickControl::joyLoop()
36	35	{
37		ros::Rate loop_rate(25);
38		while(ros::ok())
39		{
40		ros::spinOnce(); // if joyCallback is called, it will be in here
41		control.publish_on(pub);
42		loop_rate.sleep();
43		}
44		return;
	36	control.main_loop();
45	37	}
46	38
47	39	void JoystickControl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)

     #include <ros/ros.h>
     #include <mikrokopter/Control.h>
     class MKWrapper
+    {
     public:
         MKWrapper();
         void main_loop();
         void control_callback(const mikrokopter::Control& control);
     private:
         ros::Publisher pub;
         ros::Subscriber sub;
         ros::NodeHandle nh;
         ros::Duration max_quiet_time;
         ros::Time last_time_recv;
         bool started, dead;
     };
     MKWrapper::MKWrapper()
+    {
         pub = nh.advertise<mikrokopter::Control>("/mikrokopter/req_set_control",
 );
         sub = nh.subscribe("/mk_wrapper/control", 100, &MKWrapper::control_callback,
             this);
         int max_quiet_time_ms;
         ros::param::param<int>("max_quiet_time", max_quiet_time_ms, 100);
         max_quiet_time.fromSec(max_quiet_time_ms / 1000.);
         started = false;
         dead = false;
+    }
     void MKWrapper::main_loop()
+    {
         mikrokopter::Control control_dead;
         control_dead.pitch = 0;
         control_dead.roll = 0;
         control_dead.yaw = 0;
         control_dead.thrust = 60;
         ros::Rate loop_rate(20);
         while (ros::ok())
+        {
             if (started)
+            {
                 if (dead)
+                {
                     pub.publish(control_dead);
+                }
                 else if (ros::Time::now() - last_time_recv > max_quiet_time)
+                {
                     dead = true;
                     ROS_ERROR("Lost connection: mk_wrapper must be restarted");
+                }
+            }
             loop_rate.sleep();
             ros::spinOnce();
+        }
+    }
     void MKWrapper::control_callback(const mikrokopter::Control& control)
+    {
         started = true;
         if (!dead)
+        {
             pub.publish(control);
             last_time_recv = ros::Time::now();
+        }
+    }
     int main(int argc, char **argv)
+    {
         ros::init(argc, argv, "mk_wrapper");
         MKWrapper wrapper;
         wrapper.main_loop();
         return 0;
+    }

     #include "nav_lib.h"
     MikrokopterControl::MikrokopterControl()
+    {
         control.digital[0] = 0;
-...
         pub.publish(control);
+    }
     void MikrokopterControl::main_loop()
+    {
         ros::Publisher pub = nh.advertise< ::mikrokopter::Control>(
             "/mk_wrapper/control", 200);
         int publish_rate;
         ros::param::param<int>("publish_rate", publish_rate, 20);
         ros::Rate loop_rate(publish_rate);
         while (ros::ok())
+        {
             ros::spinOnce();
             pub.publish(control);
             loop_rate.sleep();
+        }
+    }

         void set_thrust(float thrust);
         void set_yaw(float yaw);
         void publish_on(ros::Publisher& pub);
         void main_loop();
     private:
         mikrokopter::Control control;
         ros::NodeHandle nh;
     };

mikrokopter/src/turn_to_target.cpp
18	18
19	19	TurnToTarget::TurnToTarget()
20	20	{
21		pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
	21	//pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
22	22	sub = n.subscribe<geometry_msgs::Point>("/v2v3_converter/target_3d", 100,
23	23	&TurnToTarget::pointCallback, this);
24	24	}
25	25
26	26	void TurnToTarget::mainLoop()
27	27	{
28		ros::Rate loop_rate(25);
	28	/*ros::Rate loop_rate(25);
29	29	while(ros::ok())
30	30	{
31	31	ros::spinOnce();
32	32	control.publish_on(pub);
33	33	loop_rate.sleep();
34		}
	34	}*/
	35	control.main_loop();
35	36	}
36	37
37	38	void TurnToTarget::pointCallback(const geometry_msgs::Point::ConstPtr& p)

Also available in: Unified diff

Project

General

Profile

Quadrotor

Revision 7a6ed02d