To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit b46d8fc5 authored by Lars's avatar Lars
Browse files

coordinate transformation

parent bedae98d
......@@ -54,6 +54,7 @@ import rosbag
from rospkg import RosPack
from std_msgs.msg import Float32
from std_msgs.msg import String
from dfall_pkg.msg import CameraData
import time, sys
import struct
......@@ -117,7 +118,7 @@ ros_namespace = rospy.get_namespace()
# Initialise a publisher for the camera
global cfcamera_pub
cfcamera_pub = rospy.Publisher(node_name, Float32, queue_size=10)
cfcamera_pub = rospy.Publisher(node_name, CameraData, queue_size=10)
......@@ -208,7 +209,8 @@ while(cam.isOpened()):
rvec = rvec[0]
tvec = tvec[0]
QueryImg = aruco.drawAxis(QueryImg, cameraMatrix, distCoeffs, rvec, tvec, 100)
cfcamera_pub.publish(0.5)
cfcamera_pub.publish(rvec[0,0],rvec[0,1],rvec[0,2],tvec[0,0],tvec[0,1],tvec[0,2])
outputr = 'r: '+str(rvec[0])
outputt = 't: '+str(tvec[0])
cv2.putText(QueryImg, outputr, (0,20), fontface, fontscale, fontcolor)
......
float64 pitch
float64 roll
float64 yaw
float64 alpha
float64 d
float64 beta
float64 x
float64 y
float64 z
......@@ -32,7 +32,7 @@
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- <build_depend>message_generlation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
......
......@@ -41,6 +41,7 @@
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
// F U U NN N C T I O O NN N
......@@ -174,6 +175,16 @@
//
//
//
float xdiff;
float ydiff;
float zdiff;
float yawdiff;
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
......@@ -186,9 +197,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
float stateErrorInertial[9];
// Fill in the (x,y,z) position error
stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0];
stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0] + xdiff/1000;
stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1] + ydiff/1000;
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2] + zdiff/1000;
// Compute an estimate of the velocity
// > As simply the derivative between the current and previous position
......@@ -695,7 +706,7 @@ void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg)
}
// Create a node handle to the selected parameter service
ros::NodeHandle nodeHandle_to_use(namespace_to_use);
// Call the function that fetches the parameters
// Call the function that fetches the parametersisReadyTemplateControllerYamlCallback
fetchTemplateControllerYamlParameters(nodeHandle_to_use);
}
}
......@@ -780,11 +791,34 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
void CrazyCameraCallback(const std_msgs::Float32 & msg)
void CrazyCameraCallback(const CameraData & msg)
{
//float a = CameraData.x;
//ROS_INFO_STREAM("CrazyCameraCallback received " << msg);
float pitch= msg.pitch;
float roll= msg.roll;
float yaw= msg.yaw;
float x= msg.x;
float y= msg.y;
float z= msg.z;
float dist=800;
ROS_INFO_STREAM("CrazyCameraCallback received " << msg.data);
xdiff=(z-dist)*sin(pitch)*cos(yaw)-y*cos(pitch)*cos(yaw)-x*sin(yaw);
ydiff=(z-dist)*cos(pitch)+y*sin(pitch);
zdiff=(z-dist)*sin(pitch)*sin(yaw)-y*cos(pitch)*sin(yaw)+x*cos(yaw);
yawdiff=yaw;
ROS_INFO_STREAM("CrazyCameraCallback received " << " "<< xdiff<< " " << " "<<" "<< ydiff <<" "<<zdiff <<" "<< yawdiff);
//m_setpoint[0] += xdiff/1000;
//m_setpoint[1] += ydiff/1000;
//m_setpoint[2] += zdiff/1000;
//value.data = msg->data;
//std::cout(value)std::endl;
//std::cout("hey")std::endl;
......
Markdown is supported
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