Commit 89698a59 authored by bucyril's avatar bucyril
Browse files

tried to get CrazyRadio running

parent 3a3ee812
......@@ -94,23 +94,20 @@ class PPSRadioClient:
def motorCommandCallback(data):
"""Callback for motor controller actions"""
rospy.loginfo("test motorCommandCallback")
cf_client._send_to_commander(0, 0, 0, 0, data.cmd1, data.cmd2, data.cmd3, 40, CONTROLLER_MOTOR)
rospy.loginfo("motor controller callback: %s, %s, %s, %s", data.cmd1,data.cmd2,data.cmd3,data.cmd4)
def angleCommandCallback(data):
"""Callback for angle controller actions"""
###cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_ANGLE)
####rospy.loginfo("angle controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
rospy.loginfo("test angleCommandCallback")
#cmd1..4 must not be 0, as crazyflie onboard controller resets
#cf_client._send_to_commander(data.rollAngle,data.pitchAngle,data.yawAngle,data.thrust, 1, 1, 1, 1, CONTROLLER_ANGLE)
rospy.loginfo("angle controller callback: %s, %s, %s, %s", data.rollAngle, data.pitchAngle ,data.yawAngle, data.thrust)
def rateCommandCallback(data):
"""Callback for rate controller actions"""
###cf_client._send_to_commander(data.roll,data.pitch,data.yaw,data.thrust, 0, 0, 0, 0, CONTROLLER_RATE)
###rospy.loginfo("rate controller callback: %s, %s, %s", data.roll,data.pitch,data.yaw,data.thrust)
rospy.loginfo("test rateCommandCallback")
def testCallback(data):
"""Callback used to test data receipt if no crazyfly was found"""
rospy.loginfo("Crazyradio.py successfully subscribed and received testvalues: %f", data.cmd1)
#cmd1..4 must not be 0, as crazyflie onboard controller resets
#cf_client._send_to_commander(data.rollRate,data.pitchRate,data.yawRate,data.thrust, 1, 1, 1 , 1, CONTROLLER_RATE)
rospy.loginfo("rate controller callback: %s, %s, %s, %s", data.rollRate, data.pitchRate, data.yawRate, data.thrust)
if __name__ == '__main__':
rospy.init_node('CrazyRadio', anonymous=True)
......@@ -119,7 +116,6 @@ if __name__ == '__main__':
while not rospy.is_shutdown():
# Scan for Crazyflies and use the first one found
rospy.loginfo("Scanning interfaces for Crazyflies...")
available=[]
......@@ -132,25 +128,15 @@ if __name__ == '__main__':
#TODO: load address from parameters
cf_client = PPSRadioClient(available[0][0])
time.sleep(3.0)
time.sleep(2.0)
#TODO: change publisher name if not correct
rospy.Subscriber("PPSClient/topicMotorCommand", MotorCommand, motorCommandCallback)
rospy.loginfo("trying to subscribe")
#rospy.Subscriber("PPSClient/AngleCommands", AngleCommand, angleCommandCallback)
#rospy.Subscriber("PPSClient/RateCommands", RateCommand, rateCommandCallback)
rospy.Subscriber("/PPSClient/MotorCommand", MotorCommand, motorCommandCallback)
rospy.Subscriber("/PPSClient/AngleCommand", AngleCommand, angleCommandCallback)
rospy.Subscriber("/PPSClient/RateCommand", RateCommand, rateCommandCallback)
rospy.spin()
rospy.loginfo("Setting crazyflie setpoint to 0")
cf_client._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, 0)
cf_client._cf.close_link()
else:
#rospy.logerr("No Crazyflies found, cannot run example")
#for testing try to subscribe even if no crazyflie was found
rospy.loginfo("No Crazyflies found, still trying to subscribe")
rospy.Subscriber("PPSClient/topicMotorCommand", MotorCommand, testCallback)
rospy.spin()
time.sleep(0.5)
cf_client._cf.close_link()
rospy.logerr("No Crazyflies found")
float32 rollAngle
float32 pitchAngle
float32 yawAngle
int32 thrust
float32 cmd1
float32 cmd2
float32 cmd3
float32 cmd4
int32 cmd1
int32 cmd2
int32 cmd3
int32 cmd4
float32 rollRate
float32 pitchRate
float32 yawRate
int32 thrust
......@@ -39,9 +39,9 @@ std::string cflie;
//global sevices
ros::ServiceClient rateClient;
ros::Publisher AngleCommandPublisher;
ros::Publisher RateCommandPublisher;
ros::Publisher MotorCommandPublisher;
ros::Publisher angleCommandPublisher;
ros::Publisher rateCommandPublisher;
ros::Publisher motorCommandPublisher;
//uncommenting the next line causes FATAL Error at runtime: "You must call ros::init() before creating the first NodeHandle"
//ros::NodeHandle nodeHandle;
......@@ -121,38 +121,40 @@ void viconCallback(const d_fall_pps::ViconData& data){
//callback method to publish d_fall_pps::AngleCommand
void callbackAngleCommand(const ros::TimerEvent&)
{
d_fall_pps::AngleCommand AngleCommandPkg;
AngleCommandPkg.rollAngle = 1;
AngleCommandPkg.pitchAngle = 1;
AngleCommandPkg.yawAngle = 1;
d_fall_pps::AngleCommand angleCommandPkg;
angleCommandPkg.rollAngle = 1;
angleCommandPkg.pitchAngle = 1;
angleCommandPkg.yawAngle = 1;
angleCommandPkg.thrust = 50;
AngleCommandPublisher.publish(AngleCommandPkg);
ROS_INFO_STREAM("AngleCommandTimer pubslishes: " << AngleCommandPkg.rollAngle << ", " << AngleCommandPkg.pitchAngle << ", " << AngleCommandPkg.yawAngle);
angleCommandPublisher.publish(angleCommandPkg);
ROS_INFO_STREAM("AngleCommandTimer pubslishes: " << angleCommandPkg.rollAngle << ", " << angleCommandPkg.pitchAngle << ", " << angleCommandPkg.yawAngle);
}
//callback method to publish d_fall_pps::RateCommand
void callbackRateCommand(const ros::TimerEvent&)
{
d_fall_pps::RateCommand RateCommandPkg;
RateCommandPkg.rollRate = 2;
RateCommandPkg.pitchRate = 2;
RateCommandPkg.yawRate = 2;
d_fall_pps::RateCommand rateCommandPkg;
rateCommandPkg.rollRate = 2;
rateCommandPkg.pitchRate = 2;
rateCommandPkg.yawRate = 2;
rateCommandPkg.thrust = 50;
RateCommandPublisher.publish(RateCommandPkg);
ROS_INFO_STREAM("RateCommandTimer pubslishes: " << RateCommandPkg.rollRate << ", " << RateCommandPkg.pitchRate << ", " << RateCommandPkg.yawRate);
rateCommandPublisher.publish(rateCommandPkg);
ROS_INFO_STREAM("RateCommandTimer pubslishes: " << rateCommandPkg.rollRate << ", " << rateCommandPkg.pitchRate << ", " << rateCommandPkg.yawRate);
}
//callback method to publish d_fall_pps::MotorCommand
void callbackMotorCommand(const ros::TimerEvent&)
{
d_fall_pps::MotorCommand MotorCommandPkg;
MotorCommandPkg.cmd1 = 3;
MotorCommandPkg.cmd2 = 3;
MotorCommandPkg.cmd3 = 3;
MotorCommandPkg.cmd4 = 3;
d_fall_pps::MotorCommand motorCommandPkg;
motorCommandPkg.cmd1 = 3;
motorCommandPkg.cmd2 = 3;
motorCommandPkg.cmd3 = 3;
motorCommandPkg.cmd4 = 3;
MotorCommandPublisher.publish(MotorCommandPkg);
ROS_INFO_STREAM("MotorCommandTimer pubslishes: " << MotorCommandPkg.cmd1 << ", " << MotorCommandPkg.cmd2 << ", " << MotorCommandPkg.cmd3 << ", " << MotorCommandPkg.cmd4);
motorCommandPublisher.publish(motorCommandPkg);
ROS_INFO_STREAM("MotorCommandTimer pubslishes: " << motorCommandPkg.cmd1 << ", " << motorCommandPkg.cmd2 << ", " << motorCommandPkg.cmd3 << ", " << motorCommandPkg.cmd4);
}
......@@ -184,15 +186,15 @@ int main(int argc, char* argv[]){
Reference: http://wiki.ros.org/roscpp/Overview/Timers
*/
ROS_INFO("creating publishers for package_for_crazyradio");
ros::Timer AngleCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackAngleCommand);
ros::Timer RateCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackRateCommand);
ros::Timer MotorCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackMotorCommand);
ros::Timer angleCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackAngleCommand);
ros::Timer rateCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackRateCommand);
ros::Timer motorCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackMotorCommand);
//ros::Publishers to advertise on the three command type topics
AngleCommandPublisher = nodeHandle.advertise <d_fall_pps::AngleCommand>("topicAngleCommand", 1000);
RateCommandPublisher = nodeHandle.advertise<d_fall_pps::RateCommand>("topicRateCommand", 1000);
MotorCommandPublisher = nodeHandle.advertise <d_fall_pps::MotorCommand>("topicMotorCommand", 1000);
angleCommandPublisher = nodeHandle.advertise <d_fall_pps::AngleCommand>("AngleCommand", 1000);
rateCommandPublisher = nodeHandle.advertise<d_fall_pps::RateCommand>("RateCommand", 1000);
motorCommandPublisher = nodeHandle.advertise <d_fall_pps::MotorCommand>("MotorCommand", 1000);
//service: now only one available: to add several services depending on controller
......
......@@ -45,6 +45,7 @@ bool calculateControlOutput(RateController::Request &request, RateController::Re
response.controlOutput.rollRate = 1; //testvalue
response.controlOutput.pitchRate = 2; //testvalue
response.controlOutput.yawRate = 3; //testvalue
response.controlOutput.thrust = 50;
return true;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment