diff --git a/src/nodes/kite_visualization_node.cpp b/src/nodes/kite_visualization_node.cpp
index 65c7d20..6b7b33f 100644
--- a/src/nodes/kite_visualization_node.cpp
+++ b/src/nodes/kite_visualization_node.cpp
@@ -1,329 +1,404 @@
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include "sensor_msgs/MultiDOFJointState.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "geometry_msgs/Point.h"
#include "geometry_msgs/Transform.h"
#include "kite.h"
using namespace casadi;
using namespace kmath;
struct Scale
{
float x;
float y;
float z;
Scale(const float &_x=1, const float &_y=1, const float &_z=1) : x(_x), y(_y), z(_z) {}
virtual ~Scale(){}
};
struct Color
{
float r;
float g;
float b;
float transparency;
Color(const float &_r=0, const float &_g=0, const float &_b=1, const float &_t=0) : r(_r), g(_g), b(_b), transparency(_t){}
virtual ~Color(){}
};
struct MarkerProperties
{
int type;
int id;
int action;
std::string frame_id;
std::string ns;
std::string mesh_resource;
Scale scale;
Color color;
geometry_msgs::Point position;
geometry_msgs::Quaternion orientation;
MarkerProperties() : type(visualization_msgs::Marker::SPHERE),
id(0),
action(visualization_msgs::Marker::ADD),
frame_id("/kite"),
ns("awe"),
scale(Scale()),
color(Color()),
mesh_resource("")
{
position.x = position.y = position.z = 0;
orientation.x = orientation.y = orientation.z = 0;
orientation.w = 1;
}
void configureMarker(visualization_msgs::Marker &marker);
};
void MarkerProperties::configureMarker(visualization_msgs::Marker &marker)
{
marker.type = type;
marker.id = id;
marker.header.frame_id = frame_id;
marker.header.stamp = ros::Time::now();
marker.action = action;
marker.color.r = color.r; marker.color.g = color.g; marker.color.b = color.b;
marker.color.a = color.transparency;
marker.scale.x = scale.x; marker.scale.y = scale.y; marker.scale.z = scale.z;
marker.pose.position = position;
marker.pose.orientation = orientation;
if(type == visualization_msgs::Marker::MESH_RESOURCE)
marker.mesh_resource = mesh_resource;
}
class KiteVisualizer
{
public:
KiteVisualizer(const ros::NodeHandle &_nh);
virtual ~KiteVisualizer(){}
- void filterCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg);
- void controlCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg);
-
geometry_msgs::Point getTranslation(){geometry_msgs::Vector3 point = kite_state.transforms[0].translation;
geometry_msgs::Point p; p.x = point.x; p.y = point.y; p.z = point.z;
return p;}
geometry_msgs::Quaternion getRotation(){return kite_state.transforms[0].rotation;}
bool is_initialized(){return initialized;}
uint32_t getNumPublishers(){return state_sub.getNumPublishers();}
DM world2rviz(const casadi::DM &pose);
visualization_msgs::Marker state2marker(const casadi::DM &kite_pose);
visualization_msgs::Marker getPose(){return m_kite_marker;}
visualization_msgs::MarkerArray getOptimalTrajectory();
visualization_msgs::Marker getVirtualTrajectory();
+ void setPath(const Function &path){PathFunction = path;}
+
private:
std::shared_ptr<ros::NodeHandle> nh;
ros::Subscriber state_sub;
ros::Subscriber opt_traj_sub;
+ void filterCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg);
+ void controlCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg);
+
sensor_msgs::MultiDOFJointState kite_state;
visualization_msgs::Marker m_kite_marker;
/** optimal trajectory visualisation */
visualization_msgs::MarkerArray kite_traj_markers;
visualization_msgs::Marker virtual_state_markers;
- sensor_msgs::MultiDOFJointState kite_trajectory;
+ DM optimal_trajectory;
+ Function PathFunction;
bool initialized;
DM convertToDM(const sensor_msgs::MultiDOFJointState &_value, const bool &with_virtual = 0);
};
KiteVisualizer::KiteVisualizer(const ros::NodeHandle &_nh)
{
nh = std::make_shared<ros::NodeHandle>(_nh);
kite_state.twist.resize(1);
kite_state.transforms.resize(1);
kite_state.joint_names.resize(1);
kite_state.header.frame_id = "kite";
kite_state.joint_names[0] = "lox";
std::string state_topic = "/kite_state";
state_sub = nh->subscribe(state_topic, 100, &KiteVisualizer::filterCallback, this);
opt_traj_sub = nh->subscribe("/opt_traj", 100, &KiteVisualizer::controlCallback, this);
/** configure kite_marker */
MarkerProperties m_props;
m_props.id = 101;
m_props.type = visualization_msgs::Marker::MESH_RESOURCE;
m_props.mesh_resource = "package://test_package/meshes/kite_rviz.dae";
m_props.scale = Scale(0.1, 0.1, 0.1);
m_props.color = Color(0.0, 1.0, 0.0, 0.5);
m_props.configureMarker(m_kite_marker);
m_kite_marker.lifetime = ros::Duration();
/** configure virtual state trajectory */
m_props.id = 201;
m_props.type = visualization_msgs::Marker::SPHERE_LIST;
m_props.color = Color(1.0, 0.0, 0.0, 0.5);
m_props.configureMarker(virtual_state_markers);
virtual_state_markers.lifetime = ros::Duration();
initialized = false;
}
void KiteVisualizer::filterCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg)
{
/** transform kite pose to Rviz reference frame */
/** @badcode: */
DM pose = convertToDM(*msg);
DM kite_state_dm = world2rviz(pose);
m_kite_marker = state2marker(kite_state_dm);
initialized = true;
}
void KiteVisualizer::controlCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg)
{
DM pose = convertToDM(*msg);
- DM kite_state_dm = world2rviz(pose);
-
- /** to finish */
+ optimal_trajectory = world2rviz(pose);
}
DM KiteVisualizer::convertToDM(const sensor_msgs::MultiDOFJointState &_value, const bool &with_virtual)
{
uint num_poses = _value.transforms.size();
int size = with_virtual ? 10 : 7;
DM poses = DM::zeros(size, num_poses);
for(uint i = 0; i < num_poses; ++i)
{
poses(0, i) = _value.transforms[i].translation.x;
poses(1, i) = _value.transforms[i].translation.y;
poses(2, i) = _value.transforms[i].translation.z;
poses(3, i) = _value.transforms[i].rotation.w;
poses(4, i) = _value.transforms[i].rotation.x;
poses(5, i) = _value.transforms[i].rotation.y;
poses(6, i) = _value.transforms[i].rotation.z;
if(with_virtual)
{
poses(7, i) = _value.wrench[i].force.x;
poses(8, i) = _value.wrench[i].force.y;
poses(9, i) = _value.wrench[i].force.z;
}
}
return poses;
}
DM KiteVisualizer::world2rviz(const DM &world_pose)
{
DM pose = DM::zeros(world_pose.size());
DM transform = DM::vertcat({0, 1, 0, 0});
+ /** do only position transform if receive a point*/
+ if(pose.size1() == 3)
+ {
+ DM q_pose = quat_multiply(transform, DM::vertcat({0, world_pose}) );
+ DM tmp = quat_multiply(q_pose, quat_inverse(transform) );
+ pose = tmp(Slice(1,4), 0);
+
+ return pose;
+ }
+
/** position transform */
DM q_pose = quat_multiply(transform, DM::vertcat({0, world_pose(Slice(0,3), 0) }) );
DM tmp = quat_multiply(q_pose, quat_inverse(transform) );
pose(Slice(0,3), 0) = tmp(Slice(1,4), 0);
/** attitude transform */
pose(Slice(3,7),0) = quat_multiply(quat_multiply(transform, world_pose(Slice(3,7), 0)), quat_inverse(transform));
if(pose.size1() == 10)
{
q_pose = quat_multiply(transform, DM::vertcat({0, world_pose(Slice(7,10), 0) }) );
tmp = quat_multiply(q_pose, quat_inverse(transform) );
pose(Slice(7,10), 0) = tmp(Slice(1,4), 0);
}
return pose;
}
+visualization_msgs::MarkerArray KiteVisualizer::getOptimalTrajectory()
+{
+ visualization_msgs::MarkerArray m_array;
+ int array_size = optimal_trajectory.size2();
+ if(array_size == 0)
+ return m_array;
+
+ DMVector split_traj = DM::horzsplit(optimal_trajectory, 1);
+
+ for(DMVector::const_iterator it = split_traj.begin(); it != split_traj.end() - 1; std::advance(it, 2))
+ {
+ /** transform to rviz ref frame */
+ DM state = world2rviz(*it);
+
+ std::vector<double> row = state.nonzeros();
+ m_kite_marker.pose.position.x = row[6];
+ m_kite_marker.pose.position.y = row[7];
+ m_kite_marker.pose.position.z = row[8];
+
+ m_kite_marker.pose.orientation.w = row[9];
+ m_kite_marker.pose.orientation.x = row[10];
+ m_kite_marker.pose.orientation.y = row[11];
+ m_kite_marker.pose.orientation.z = row[12];
+
+ m_array.markers.push_back(m_kite_marker);
+ }
+ return m_array;
+}
+
+visualization_msgs::Marker KiteVisualizer::getVirtualTrajectory()
+{
+ visualization_msgs::Marker m_array;
+ int array_size = optimal_trajectory.size2();
+ if(array_size == 0 || PathFunction.is_null())
+ return m_array;
+
+ DMVector split_traj = DM::horzsplit(optimal_trajectory, 1);
+ virtual_state_markers.points.clear();
+
+ for(DMVector::const_iterator it = split_traj.begin(); it != split_traj.end(); std::advance(it, 1))
+ {
+ DM theta = (*it)(13);
+ DMVector res = PathFunction(DMVector{theta});
+ DM point = world2rviz(res[0]);
+ geometry_msgs::Point p;
+ std::vector<double> coords = point.nonzeros();
+ p.x = coords[0];
+ p.y = coords[1];
+ p.z = coords[2];
+
+ virtual_state_markers.points.push_back(p);
+ }
+ return m_array;
+}
+
visualization_msgs::Marker KiteVisualizer::state2marker(const DM &kite_pose)
{
visualization_msgs::Marker marker;
std::vector<double> pose = kite_pose.nonzeros();
marker.pose.position.x = pose[0];
marker.pose.position.y = pose[1];
marker.pose.position.z = pose[2];
marker.pose.orientation.w = pose[3];
marker.pose.orientation.x = pose[4];
marker.pose.orientation.y = pose[5];
marker.pose.orientation.z = pose[6];
return marker;
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "kite_visualization_node");
ros::NodeHandle n;
ros::Rate r(10);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
ros::Publisher trajec_pub = n.advertise<visualization_msgs::MarkerArray>("trajectory_marker", 10);
KiteVisualizer kite(n);
visualization_msgs::Marker gs_marker;
visualization_msgs::Marker kite_marker;
visualization_msgs::Marker path_marker;
MarkerProperties marker_props;
marker_props.type = visualization_msgs::Marker::CYLINDER;
marker_props.scale = Scale(0.1, 0.1, 2.0);
marker_props.color = Color(0.0, 1.0, 0.0, 1.0);
marker_props.configureMarker(gs_marker);
gs_marker.lifetime = ros::Duration();
/** kite marker set up */
marker_props.id = 1;
marker_props.type = visualization_msgs::Marker::MESH_RESOURCE;
marker_props.mesh_resource = "package://test_package/meshes/kite_rviz.dae";
marker_props.scale = Scale(0.1, 0.1, 0.1);
marker_props.configureMarker(kite_marker);
kite_marker.lifetime = ros::Duration();
/** path markers set up */
marker_props.id = 3;
marker_props.type = visualization_msgs::Marker::LINE_STRIP;
marker_props.scale.x = 0.025;
marker_props.configureMarker(path_marker);
path_marker.lifetime = ros::Duration();
/** create a path */
SX x = SX::sym("x");
double radius = 2.31;
double altitude = 3.0;
SX Path = SX::vertcat(SXVector{radius * cos(x), radius * sin(x), altitude});
Function path_fun = Function("path", {x}, {Path});
for(double alpha = 0; alpha < 2 * M_PI; alpha += 0.2)
{
DMVector res = path_fun( DMVector{DM(alpha)} );
geometry_msgs::Point p;
DM point = res[0];
p.x = point.nonzeros()[0];
p.y = point.nonzeros()[1];
p.z = point.nonzeros()[2];
path_marker.points.push_back(p);
}
bool published_once = false;
while (ros::ok())
{
gs_marker.lifetime = ros::Duration();
kite_marker.lifetime = ros::Duration();
if(kite.is_initialized())
{
kite_marker = kite.getPose();
}
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1);
}
marker_pub.publish(gs_marker);
marker_pub.publish(kite_marker);
+ /** draw optimal trajectories */
+ visualization_msgs::Marker virt_markers = kite.getVirtualTrajectory();
+ visualization_msgs::MarkerArray opt_markers = kite.getOptimalTrajectory();
+ if(!opt_markers.markers.empty())
+ trajec_pub.publish(opt_markers);
+
+ if(!virt_markers.points.empty())
+ marker_pub.publish(virt_markers);
+
if(!published_once)
{
marker_pub.publish(path_marker);
published_once = true;
}
ros::spinOnce();
r.sleep();
}
}