Skip to content

Commit

Permalink
Merge pull request #3 from ethz-asl/fix/rviz-plugins
Browse files Browse the repository at this point in the history 
fixed rviz plugins GUI update, works now with rqt_rviz
  • Loading branch information
@remod
remod committed Sep 18, 2015
2 parents 03acf95 + ce49f6a commit ae8b889
Show file tree
Hide file tree
Showing 5 changed files with 153 additions and 122 deletions.
4 changes: 4 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,3 +26,7 @@
*.exe
*.out
*.app

# CLion
.idea/

7 changes: 7 additions & 0 deletions kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,13 +65,18 @@ Q_OBJECT
// A helper to clear this display back to the initial state.
virtual void reset();

Q_SIGNALS:
void updateVectorAtPositionSignal();

// These Qt slots get connected to signals indicating changes in the user-editable properties.
private Q_SLOTS:
void updateScale();
void updateShowText();
void updateColorAndAlpha();
void updateHistoryLength();

void updateVectorAtPosition();

// Function to handle an incoming ROS message.
private:
void processMessage(const kindr_msgs::VectorAtPosition::ConstPtr& msg);
Expand All @@ -94,6 +99,8 @@ private Q_SLOTS:
bool showText_;
Ogre::ColourValue color_;
float alpha_;

kindr_msgs::VectorAtPosition::ConstPtr current_vector_at_position_;
};

} // kindr_rviz_plugins
32 changes: 20 additions & 12 deletions kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ VectorAtPositionDisplay::VectorAtPositionDisplay()
color_(Ogre::ColourValue::Black),
alpha_(1.0)
{
connect(this, SIGNAL(updateVectorAtPositionSignal()), this, SLOT(updateVectorAtPosition()));

length_scale_property_ = new rviz::FloatProperty("Length scale", 1.0,
"Scale of the length of the vector.",
this, SLOT(updateScale()));
Expand Down Expand Up @@ -124,43 +126,42 @@ void VectorAtPositionDisplay::updateHistoryLength()
visuals_.rset_capacity(history_length_property_->getInt());
}

// This is our callback to handle an incoming message.
void VectorAtPositionDisplay::processMessage(const kindr_msgs::VectorAtPosition::ConstPtr& msg)
{
void VectorAtPositionDisplay::updateVectorAtPosition() {

// Here we call the rviz::FrameManager to get the transform from the
// fixed frame to the frame in the header of this VectorAtPosition message.
Ogre::Vector3 arrowPosition;
Ogre::Quaternion arrowOrientation;

// Check if the position has an empty or the same frame as the vector
if (msg->position_frame_id.empty() || msg->position_frame_id == msg->header.frame_id)
if (current_vector_at_position_->position_frame_id.empty() || current_vector_at_position_->position_frame_id == current_vector_at_position_->header.frame_id)
{
// Get arrow position and orientation
if(!context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, arrowPosition, arrowOrientation))
if(!context_->getFrameManager()->getTransform(current_vector_at_position_->header.frame_id, current_vector_at_position_->header.stamp, arrowPosition, arrowOrientation))
{
ROS_ERROR("Error transforming from frame '%s' to frame '%s'", msg->position_frame_id.c_str(), qPrintable(fixed_frame_));
ROS_ERROR("Error transforming from frame '%s' to frame '%s'", current_vector_at_position_->position_frame_id.c_str(), qPrintable(fixed_frame_));
return;
}
}
else
{
// Get arrow position
Ogre::Quaternion dummyOrientation;
if(!context_->getFrameManager()->getTransform(msg->position_frame_id, msg->header.stamp, arrowPosition, dummyOrientation))
if(!context_->getFrameManager()->getTransform(current_vector_at_position_->position_frame_id, current_vector_at_position_->header.stamp, arrowPosition, dummyOrientation))
{
ROS_ERROR("Error transforming from frame '%s' to frame '%s'", msg->position_frame_id.c_str(), qPrintable(fixed_frame_));
ROS_ERROR("Error transforming from frame '%s' to frame '%s'", current_vector_at_position_->position_frame_id.c_str(), qPrintable(fixed_frame_));
return;
}

// Get arrow orientation
Ogre::Vector3 dummyPosition;
if(!context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, dummyPosition, arrowOrientation))
if(!context_->getFrameManager()->getTransform(current_vector_at_position_->header.frame_id, current_vector_at_position_->header.stamp, dummyPosition, arrowOrientation))
{
ROS_ERROR("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
ROS_ERROR("Error transforming from frame '%s' to frame '%s'", current_vector_at_position_->header.frame_id.c_str(), qPrintable(fixed_frame_));
return;
}
}
arrowPosition += Ogre::Vector3(msg->position.x, msg->position.y, msg->position.z);
arrowPosition += Ogre::Vector3(current_vector_at_position_->position.x, current_vector_at_position_->position.y, current_vector_at_position_->position.z);

// We are keeping a circular buffer of visual pointers. This gets
// the next one, or creates and stores it if the buffer is not full
Expand All @@ -175,7 +176,7 @@ void VectorAtPositionDisplay::processMessage(const kindr_msgs::VectorAtPosition:
}

// Now set or update the contents of the chosen visual.
visual->setMessage(msg);
visual->setMessage(current_vector_at_position_);
visual->setArrowPosition(arrowPosition); // position is taken from position in msg
visual->setArrowOrientation(arrowOrientation); // orientation is taken from vector in msg
visual->setScalingFactors(lengthScale_, widthScale_);
Expand All @@ -186,6 +187,13 @@ void VectorAtPositionDisplay::processMessage(const kindr_msgs::VectorAtPosition:
visuals_.push_back(visual);
}

// This is our callback to handle an incoming message.
void VectorAtPositionDisplay::processMessage(const kindr_msgs::VectorAtPosition::ConstPtr& msg)
{
current_vector_at_position_ = msg;
Q_EMIT updateVectorAtPositionSignal();
}

} // kindr_rviz_plugins


Expand Down
5 changes: 5 additions & 0 deletions ...lugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,8 @@ Q_OBJECT
virtual void onInitialize();
virtual void reset();

Q_SIGNALS:
void updateTrajectorySignal();

private Q_SLOTS:
void setShowConnection();
Expand Down Expand Up @@ -68,6 +70,7 @@ private Q_SLOTS:

void setHistoryLength();

void updateTrajectory();

private:
void processMessage(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg);
Expand Down Expand Up @@ -149,6 +152,8 @@ private Q_SLOTS:

float font_size_;
bool show_text_;

trajectory_msgs::MultiDOFJointTrajectory::ConstPtr current_trajectory_;
};

} // multi_dof_joint_trajectory_rviz_plugins
227 changes: 117 additions & 110 deletions multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@ MultiDOFJointTrajectoryDisplay::MultiDOFJointTrajectoryDisplay()
font_size_(0.05),
show_text_(true)
{
connect(this, SIGNAL(updateTrajectorySignal()), this, SLOT(updateTrajectory()));

property_show_connection_ = new rviz::BoolProperty(
"Show Connections", show_connection_,
"Enable or disable connections rendering.",
Expand Down Expand Up @@ -300,123 +302,128 @@ void MultiDOFJointTrajectoryDisplay::setHistoryLength()
visuals_connections_.rset_capacity(property_history_length_->getInt());
}

void MultiDOFJointTrajectoryDisplay::processMessage(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg)
{
show_connection_ = property_show_connection_->getBool();
show_transform_rotation_ = property_show_transform_rotation_->getBool();
show_velocity_linear_ = property_show_velocity_linear_->getBool();
show_velocity_angular_ = property_show_velocity_angular_->getBool();
show_acceleration_linear_ = property_show_acceleration_linear_->getBool();
show_acceleration_angular_ = property_show_acceleration_angular_->getBool();
void MultiDOFJointTrajectoryDisplay::updateTrajectory() {
show_connection_ = property_show_connection_->getBool();
show_transform_rotation_ = property_show_transform_rotation_->getBool();
show_velocity_linear_ = property_show_velocity_linear_->getBool();
show_velocity_angular_ = property_show_velocity_angular_->getBool();
show_acceleration_linear_ = property_show_acceleration_linear_->getBool();
show_acceleration_angular_ = property_show_acceleration_angular_->getBool();

size_transform_rotation_ = property_size_transform_rotation_->getFloat();
scale_velocity_linear_ = property_scale_velocity_linear_->getFloat();
scale_velocity_angular_ = property_scale_velocity_angular_->getFloat();
scale_acceleration_linear_ = property_scale_acceleration_linear_->getFloat();
scale_acceleration_angular_ = property_scale_acceleration_angular_->getFloat();

color_connection_ = rviz::qtToOgre(property_color_connection_->getColor());
color_velocity_linear_ = rviz::qtToOgre(property_color_velocity_linear_->getColor());
color_velocity_angular_ = rviz::qtToOgre(property_color_velocity_angular_->getColor());
color_acceleration_linear_ = rviz::qtToOgre(property_color_acceleration_linear_->getColor());
color_acceleration_angular_ = rviz::qtToOgre(property_color_acceleration_angular_->getColor());
alpha_ = property_alpha_->getFloat();
color_connection_.a = alpha_;
color_velocity_linear_.a = alpha_;
color_velocity_angular_.a = alpha_;
color_acceleration_linear_.a = alpha_;
color_acceleration_angular_.a = alpha_;

std::vector<std::vector<std::string>> captions;
for (unsigned int i = 0; i < current_trajectory_->points.size(); i++)
{
std::vector<std::string> caption_point;
for (unsigned int j = 0; j < current_trajectory_->joint_names.size(); j++)
{
std::stringstream ss;
ss << current_trajectory_->joint_names[j] << ": t" << i << " = " << current_trajectory_->points[i].time_from_start.toSec() << "s";
caption_point.push_back(ss.str());
}
captions.push_back(caption_point);
}

size_transform_rotation_ = property_size_transform_rotation_->getFloat();
scale_velocity_linear_ = property_scale_velocity_linear_->getFloat();
scale_velocity_angular_ = property_scale_velocity_angular_->getFloat();
scale_acceleration_linear_ = property_scale_acceleration_linear_->getFloat();
scale_acceleration_angular_ = property_scale_acceleration_angular_->getFloat();
font_size_ = property_font_size_->getFloat();
show_text_ = property_show_text_->getBool();

color_connection_ = rviz::qtToOgre(property_color_connection_->getColor());
color_velocity_linear_ = rviz::qtToOgre(property_color_velocity_linear_->getColor());
color_velocity_angular_ = rviz::qtToOgre(property_color_velocity_angular_->getColor());
color_acceleration_linear_ = rviz::qtToOgre(property_color_acceleration_linear_->getColor());
color_acceleration_angular_ = rviz::qtToOgre(property_color_acceleration_angular_->getColor());
alpha_ = property_alpha_->getFloat();
color_connection_.a = alpha_;
color_velocity_linear_.a = alpha_;
color_velocity_angular_.a = alpha_;
color_acceleration_linear_.a = alpha_;
color_acceleration_angular_.a = alpha_;
std::vector<boost::shared_ptr<MultiDOFJointTrajectoryPointVisual>> visuals_points;
std::vector<boost::shared_ptr<MultiDOFJointTrajectoryPointConnectionVisual>> visuals_connections;

std::vector<std::vector<std::string>> captions;
for (unsigned int i = 0; i < msg->points.size(); i++)
{
std::vector<std::string> caption_point;
for (unsigned int j = 0; j < msg->joint_names.size(); j++)
trajectory_msgs::MultiDOFJointTrajectoryPoint last_point;
trajectory_msgs::MultiDOFJointTrajectoryPoint current_point = current_trajectory_->points[0];

// add first point
visuals_points.push_back(boost::shared_ptr<MultiDOFJointTrajectoryPointVisual>(new MultiDOFJointTrajectoryPointVisual(
context_->getSceneManager(),
scene_node_,
current_point,
show_transform_rotation_,
show_velocity_linear_,
show_velocity_angular_,
show_acceleration_linear_,
show_acceleration_angular_,
size_transform_rotation_,
diameter_arrows_,
scale_velocity_linear_,
scale_velocity_angular_,
scale_acceleration_linear_,
scale_acceleration_angular_,
alpha_,
color_velocity_linear_,
color_velocity_angular_,
color_acceleration_linear_,
color_acceleration_angular_,
captions[0],
font_size_,
show_text_)));

// add second to last points and connections to predecessors
for (unsigned int i = 1; i < current_trajectory_->points.size(); i++)
{
std::stringstream ss;
ss << msg->joint_names[j] << ": t" << i << " = " << msg->points[i].time_from_start.toSec() << "s";
caption_point.push_back(ss.str());
// go one pose further
last_point = current_point;
current_point = current_trajectory_->points[i];

// add edge to predecessor
visuals_connections.push_back(boost::shared_ptr<MultiDOFJointTrajectoryPointConnectionVisual>(new MultiDOFJointTrajectoryPointConnectionVisual(context_->getSceneManager(),
scene_node_,
last_point,
current_point,
show_connection_,
color_connection_)));

// add pose
visuals_points.push_back(boost::shared_ptr<MultiDOFJointTrajectoryPointVisual>(new MultiDOFJointTrajectoryPointVisual(
context_->getSceneManager(),
scene_node_,
current_point,
show_transform_rotation_,
show_velocity_linear_,
show_velocity_angular_,
show_acceleration_linear_,
show_acceleration_angular_,
size_transform_rotation_,
diameter_arrows_,
scale_velocity_linear_,
scale_velocity_angular_,
scale_acceleration_linear_,
scale_acceleration_angular_,
alpha_,
color_velocity_linear_,
color_velocity_angular_,
color_acceleration_linear_,
color_acceleration_angular_,
captions[i],
font_size_,
show_text_)));
}
captions.push_back(caption_point);
}

font_size_ = property_font_size_->getFloat();
show_text_ = property_show_text_->getBool();

std::vector<boost::shared_ptr<MultiDOFJointTrajectoryPointVisual>> visuals_points;
std::vector<boost::shared_ptr<MultiDOFJointTrajectoryPointConnectionVisual>> visuals_connections;

trajectory_msgs::MultiDOFJointTrajectoryPoint last_point;
trajectory_msgs::MultiDOFJointTrajectoryPoint current_point = msg->points[0];

// add first point
visuals_points.push_back(boost::shared_ptr<MultiDOFJointTrajectoryPointVisual>(new MultiDOFJointTrajectoryPointVisual(
context_->getSceneManager(),
scene_node_,
current_point,
show_transform_rotation_,
show_velocity_linear_,
show_velocity_angular_,
show_acceleration_linear_,
show_acceleration_angular_,
size_transform_rotation_,
diameter_arrows_,
scale_velocity_linear_,
scale_velocity_angular_,
scale_acceleration_linear_,
scale_acceleration_angular_,
alpha_,
color_velocity_linear_,
color_velocity_angular_,
color_acceleration_linear_,
color_acceleration_angular_,
captions[0],
font_size_,
show_text_)));

// add second to last points and connections to predecessors
for (unsigned int i = 1; i < msg->points.size(); i++)
{
// go one pose further
last_point = current_point;
current_point = msg->points[i];

// add edge to predecessor
visuals_connections.push_back(boost::shared_ptr<MultiDOFJointTrajectoryPointConnectionVisual>(new MultiDOFJointTrajectoryPointConnectionVisual(context_->getSceneManager(),
scene_node_,
last_point,
current_point,
show_connection_,
color_connection_)));

// add pose
visuals_points.push_back(boost::shared_ptr<MultiDOFJointTrajectoryPointVisual>(new MultiDOFJointTrajectoryPointVisual(
context_->getSceneManager(),
scene_node_,
current_point,
show_transform_rotation_,
show_velocity_linear_,
show_velocity_angular_,
show_acceleration_linear_,
show_acceleration_angular_,
size_transform_rotation_,
diameter_arrows_,
scale_velocity_linear_,
scale_velocity_angular_,
scale_acceleration_linear_,
scale_acceleration_angular_,
alpha_,
color_velocity_linear_,
color_velocity_angular_,
color_acceleration_linear_,
color_acceleration_angular_,
captions[i],
font_size_,
show_text_)));
}
visuals_points_.push_back(visuals_points);
visuals_connections_.push_back(visuals_connections);
}

visuals_points_.push_back(visuals_points);
visuals_connections_.push_back(visuals_connections);
void MultiDOFJointTrajectoryDisplay::processMessage(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg)
{
current_trajectory_ = msg;
Q_EMIT updateTrajectorySignal();
}

void MultiDOFJointTrajectoryDisplay::updateShowConnection()
Expand Down

0 comments on commit ae8b889

Please sign in to comment.