Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added status topic to indicate which mux channel is selected and if l… #20

Closed
wants to merge 3 commits into from
Closed

Added status topic to indicate which mux channel is selected and if l… #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d560090
Added status topic to indicate which mux channel is selected and if l…
May 28, 2020
7149619
Fixed bracket style
Michael-Equi Jul 13, 2020
8829d2b
Removed edit
Michael-Equi Jul 13, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion include/twist_mux/topic_handle.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,7 +169,7 @@ class VelocityTopicHandle : public TopicHandle_<geometry_msgs::Twist>
// all the topic list; so far there's no O(1) solution.
if (mux_->hasPriority(*this))
{
mux_->publishTwist(msg);
mux_->publishTwist(msg, name_);
}
}
};
Expand Down
7 changes: 5 additions & 2 deletions include/twist_mux/twist_mux.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>

#include <list>

Expand Down Expand Up @@ -57,7 +58,7 @@ class TwistMux

bool hasPriority(const VelocityTopicHandle& twist);

void publishTwist(const geometry_msgs::TwistConstPtr& msg);
void publishTwist(const geometry_msgs::TwistConstPtr& msg, const std::string& name);

void updateDiagnostics(const ros::TimerEvent& event);

Expand All @@ -82,7 +83,9 @@ class TwistMux
boost::shared_ptr<velocity_topic_container> velocity_hs_;
boost::shared_ptr<lock_topic_container> lock_hs_;

ros::Publisher cmd_pub_;
ros::Publisher cmd_pub_, status_pub_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

wouldn't it be handier to add a new message that contains these two strings and publish that?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I usually try to avoid adding custom messages for stuff like this. Unless you really believe that a new message type is the best way to go and don't like the current implementation of two topics, I'm thinking it might be better to either concatenate the strings with an easily parse-able delimiter or potentially use something like the diagnostics messages.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I made the other couple small changes, let me know what you think the best course of action is on this. Unfortunately I no longer have a melodic setup with ROS1 so I may not be able to make + test any major modifications anytime soon.

bool pub_status_continuously_;
std::string last_pub_name_, last_locked_name_;

geometry_msgs::Twist last_cmd_;

Expand Down
2 changes: 2 additions & 0 deletions launch/twist_mux.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@
<node pkg="twist_mux" type="twist_mux" name="twist_mux" output="screen">
<remap from="cmd_vel_out" to="$(arg cmd_vel_out)"/>

<param name="pub_status_continuously" value="false"/>

<rosparam file="$(arg config_locks)" command="load"/>
<rosparam file="$(arg config_topics)" command="load"/>
</node>
Expand Down
22 changes: 20 additions & 2 deletions src/twist_mux.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
#include <twist_mux/topic_handle.h>
#include <twist_mux/twist_mux_diagnostics.h>
#include <twist_mux/twist_mux_diagnostics_status.h>
#include <twist_mux/utils.h>
#include <twist_mux/xmlrpc_helpers.h>

/**
Expand Down Expand Up @@ -53,6 +52,9 @@ TwistMux::TwistMux(int window_size)
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");

/// Get basic node configuration
nh_priv.param("pub_status_continuously", pub_status_continuously_, false);

/// Get topics and locks:
velocity_hs_ = boost::make_shared<velocity_topic_container>();
lock_hs_ = boost::make_shared<lock_topic_container>();
Expand All @@ -61,6 +63,7 @@ TwistMux::TwistMux(int window_size)

/// Publisher for output topic:
cmd_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel_out", 1);
status_pub_ = nh.advertise<std_msgs::String>("out_status", 5, true);
Copy link

@Timple Timple Jun 2, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just "status" is enough, "status in" is quite rare for nodes.


/// Diagnostics:
diagnostics_ = boost::make_shared<diagnostics_type>();
Expand All @@ -80,9 +83,15 @@ void TwistMux::updateDiagnostics(const ros::TimerEvent& event)
diagnostics_->updateStatus(status_);
}

void TwistMux::publishTwist(const geometry_msgs::TwistConstPtr& msg)
void TwistMux::publishTwist(const geometry_msgs::TwistConstPtr& msg, const std::string& name)
{
cmd_pub_.publish(*msg);
if(pub_status_continuously_ || name != last_pub_name_){
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also here, { on next line

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, I see here why the param is appended with continuously. Sorry I missed that part, than the parameter name does make sense!

But why publish a topic continuously when the data does not change? This is exactly what latched topics are for!

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That mostly had to do with my application at the moment of writing this. I had it going through a ROS1 bridge to ROS2 which meant latching did not work for me so I had to get around it with that solution. I can probably remove it now since that specific application is no longer something I need.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, it's probably nicer not to modify all nodes for a shortcoming in
the bridge :)

std_msgs::String status;
status.data = name;
status_pub_.publish(status);
last_pub_name_ = name;
}
}

template<typename T>
Expand Down Expand Up @@ -157,6 +166,15 @@ bool TwistMux::hasPriority(const VelocityTopicHandle& twist)
priority = velocity_priority;
velocity_name = velocity_h.getName();
}
}
else if(last_pub_name_ == velocity_h.getName() &&
(pub_status_continuously_ || last_locked_name_ != velocity_h.getName()))
{
std_msgs::String status;
status.data = "locked";
last_pub_name_ = "locked";
status_pub_.publish(status);
last_locked_name_ = velocity_h.getName();
}
}

Expand Down