diff --git a/.gitignore b/.gitignore index b8bd026..8f1a05b 100644 --- a/.gitignore +++ b/.gitignore @@ -26,3 +26,7 @@ *.exe *.out *.app + +# CLion +.idea/ + diff --git a/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp b/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp index a9a4b89..39805a3 100644 --- a/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp +++ b/kindr_rviz_plugins/include/kindr_rviz_plugins/VectorAtPositionDisplay.hpp @@ -65,6 +65,9 @@ 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(); @@ -72,6 +75,8 @@ private Q_SLOTS: void updateColorAndAlpha(); void updateHistoryLength(); + void updateVectorAtPosition(); + // Function to handle an incoming ROS message. private: void processMessage(const kindr_msgs::VectorAtPosition::ConstPtr& msg); @@ -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 diff --git a/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp b/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp index 625670b..d0d6085 100644 --- a/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp +++ b/kindr_rviz_plugins/src/VectorAtPositionDisplay.cpp @@ -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())); @@ -124,21 +126,20 @@ 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; } } @@ -146,21 +147,21 @@ void VectorAtPositionDisplay::processMessage(const kindr_msgs::VectorAtPosition: { // 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 @@ -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_); @@ -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 diff --git a/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp index dc4e0a6..ffc2066 100644 --- a/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp +++ b/multi_dof_joint_trajectory_rviz_plugins/include/multi_dof_joint_trajectory_rviz_plugins/MultiDOFJointTrajectoryDisplay.hpp @@ -40,6 +40,8 @@ Q_OBJECT virtual void onInitialize(); virtual void reset(); +Q_SIGNALS: + void updateTrajectorySignal(); private Q_SLOTS: void setShowConnection(); @@ -68,6 +70,7 @@ private Q_SLOTS: void setHistoryLength(); + void updateTrajectory(); private: void processMessage(const trajectory_msgs::MultiDOFJointTrajectory::ConstPtr& msg); @@ -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 diff --git a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp index 76df894..41b22f5 100644 --- a/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp +++ b/multi_dof_joint_trajectory_rviz_plugins/src/MultiDOFJointTrajectoryDisplay.cpp @@ -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.", @@ -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> captions; + for (unsigned int i = 0; i < current_trajectory_->points.size(); i++) + { + std::vector 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> visuals_points; + std::vector> visuals_connections; - std::vector> captions; - for (unsigned int i = 0; i < msg->points.size(); i++) - { - std::vector 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(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(new MultiDOFJointTrajectoryPointConnectionVisual(context_->getSceneManager(), + scene_node_, + last_point, + current_point, + show_connection_, + color_connection_))); + + // add pose + visuals_points.push_back(boost::shared_ptr(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> visuals_points; - std::vector> 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(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(new MultiDOFJointTrajectoryPointConnectionVisual(context_->getSceneManager(), - scene_node_, - last_point, - current_point, - show_connection_, - color_connection_))); - - // add pose - visuals_points.push_back(boost::shared_ptr(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()