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

Jump to bottom

Added additional degrees of freedom (YZ) to linear interactive markers. #7

Merged
merged 4 commits into from
Jun 20, 2016
Merged

Added additional degrees of freedom (YZ) to linear interactive markers. #7

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
4b7115b
Added additional degrees of freedom (YZ) to linear interactive markers.
tonybaltovski Jul 8, 2015
f2570d9
Fixed typo.
tonybaltovski Jul 8, 2015
f370fbf
Consolidated launch files.
tonybaltovski Jul 8, 2015
c9eddba
Renamed config from holonomic to planar.
tonybaltovski Jul 16, 2015
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
4 changes: 4 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,10 @@ install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
if(CATKIN_ENABLE_TESTING)
roslaunch_add_file_check(launch/interactive_markers.launch
DEPENDENCIES marker_server)
roslaunch_add_file_check(launch/interactive_markers_aerial.launch
DEPENDENCIES marker_server)
roslaunch_add_file_check(launch/interactive_markers_holonomic.launch
DEPENDENCIES marker_server)
roslint_cpp()
roslint_add_test()
endif()
12 changes: 12 additions & 0 deletions config/aerial.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
linear_scale:
x: 1
y: 1
z: 1
max_positive_linear_velocity:
x: 1
y: 1
z: 1
max_negative_linear_velocity:
x: -1
y: -1
z: -1
9 changes: 9 additions & 0 deletions config/holonomic.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
linear_scale:
x: 1
y: 1
max_positive_linear_velocity:
x: 1
y: 1
max_negative_linear_velocity:
x: -1
y: -1
5 changes: 5 additions & 0 deletions launch/interactive_markers_aerial.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
<launch>
<node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server">
<rosparam command="load" file="$(find interactive_marker_twist_server)/config/aerial.yaml" />
Copy link
Member

Choose a reason for hiding this comment

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

Rather than creating multiple files here, just use an $(arg config) with a sensible default. (perhaps create a diffdrive.yaml or something)

</node>
</launch>
5 changes: 5 additions & 0 deletions launch/interactive_markers_holonomic.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
<launch>
<node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server">
<rosparam command="load" file="$(find interactive_marker_twist_server)/config/holonomic.yaml" />
</node>
</launch>
104 changes: 73 additions & 31 deletions src/marker_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@

#include <algorithm>
#include <string>
#include <map>

using visualization_msgs::InteractiveMarker;
using visualization_msgs::InteractiveMarkerControl;
Expand All @@ -52,13 +53,22 @@ class MarkerServer
nh.param<std::string>("link_name", link_name, "/base_link");
nh.param<std::string>("robot_name", robot_name, "robot");

nh.param<double>("linear_scale", linear_drive_scale, 1.0);
nh.param<double>("angular_scale", angular_drive_scale, 2.2);
nh.param<double>("marker_size_scale", marker_size_scale, 1.0);
if (nh.getParam("linear_scale", linear_drive_scale_map))
{
nh.getParam("linear_scale", linear_drive_scale_map);
nh.getParam("max_positive_linear_velocity", max_positive_linear_velocity_map);
nh.getParam("max_negative_linear_velocity", max_negative_linear_velocity_map);
}
else
{
nh.param<double>("linear_scale", linear_drive_scale_map["x"], 1.0);
nh.param<double>("max_positive_linear_velocity", max_positive_linear_velocity_map["x"], 1.0);
nh.param<double>("max_negative_linear_velocity", max_negative_linear_velocity_map["x"], -1.0);
}

nh.param<double>("max_positive_linear_velocity", max_positive_linear_velocity, 1.0);
nh.param<double>("max_negative_linear_velocity", max_negative_linear_velocity, -1.0);
nh.param<double>("angular_scale", angular_drive_scale, 2.2);
nh.param<double>("max_angular_velocity", max_angular_velocity, 2.2);
nh.param<double>("marker_size_scale", marker_size_scale, 1.0);

vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
createInteractiveMarkers();
Expand All @@ -76,10 +86,11 @@ class MarkerServer
ros::Publisher vel_pub;
interactive_markers::InteractiveMarkerServer server;

double linear_drive_scale;
std::map<std::string, double> linear_drive_scale_map;
std::map<std::string, double> max_positive_linear_velocity_map;
std::map<std::string, double> max_negative_linear_velocity_map;

double angular_drive_scale;
double max_positive_linear_velocity;
double max_negative_linear_velocity;
double max_angular_velocity;
double marker_size_scale;

Expand All @@ -90,19 +101,33 @@ class MarkerServer
void MarkerServer::processFeedback(
const InteractiveMarkerFeedback::ConstPtr &feedback )
{
geometry_msgs::Twist vel;

// Handle angular change (yaw is the only direction in which you can rotate)
double yaw = tf::getYaw(feedback->pose.orientation);

geometry_msgs::Twist vel;
vel.angular.z = angular_drive_scale * yaw;
vel.linear.x = linear_drive_scale * feedback->pose.position.x;

// Enforce parameterized speed limits
vel.linear.x = std::min(vel.linear.x, max_positive_linear_velocity);
vel.linear.x = std::max(vel.linear.x, max_negative_linear_velocity);
vel.angular.z = std::min(vel.angular.z, max_angular_velocity);
vel.angular.z = std::min(vel.angular.z, max_angular_velocity);
vel.angular.z = std::max(vel.angular.z, -max_angular_velocity);

if (linear_drive_scale_map.find("x") != linear_drive_scale_map.end())
{
vel.linear.x = linear_drive_scale_map["x"] * feedback->pose.position.x;
vel.linear.x = std::min(vel.linear.x, max_positive_linear_velocity_map["x"]);
vel.linear.x = std::max(vel.linear.x, max_negative_linear_velocity_map["x"]);
}
if (linear_drive_scale_map.find("y") != linear_drive_scale_map.end())
{
vel.linear.y = linear_drive_scale_map["y"] * feedback->pose.position.y;
vel.linear.y = std::min(vel.linear.y, max_positive_linear_velocity_map["y"]);
vel.linear.y = std::max(vel.linear.y, max_negative_linear_velocity_map["y"]);
}
if (linear_drive_scale_map.find("z") != linear_drive_scale_map.end())
{
vel.linear.z = linear_drive_scale_map["z"] * feedback->pose.position.z;
vel.linear.z = std::min(vel.linear.z, max_positive_linear_velocity_map["z"]);
vel.linear.z = std::max(vel.linear.z, max_negative_linear_velocity_map["z"]);
}

vel_pub.publish(vel);

// Make the marker snap back to robot
Expand All @@ -123,13 +148,37 @@ void MarkerServer::createInteractiveMarkers()
InteractiveMarkerControl control;

control.orientation_mode = InteractiveMarkerControl::FIXED;
control.orientation.w = 1;
control.orientation.x = 1;
control.orientation.y = 0;
control.orientation.z = 0;
control.name = "move_x";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);

if (linear_drive_scale_map.find("x") != linear_drive_scale_map.end())
{
control.orientation.w = 1;
control.orientation.x = 1;
control.orientation.y = 0;
control.orientation.z = 0;
control.name = "move_x";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
}
if (linear_drive_scale_map.find("y") != linear_drive_scale_map.end())
{
control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 0;
control.orientation.z = 1;
control.name = "move_y";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
}
if (linear_drive_scale_map.find("z") != linear_drive_scale_map.end())
{
control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 1;
control.orientation.z = 0;
control.name = "move_z";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);
}

control.orientation.w = 1;
control.orientation.x = 0;
Expand All @@ -139,14 +188,7 @@ void MarkerServer::createInteractiveMarkers()
control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
int_marker.controls.push_back(control);

// Commented out for non-holonomic robot. If holonomic, can move in y.
/*control.orientation.w = 1;
control.orientation.x = 0;
control.orientation.y = 0;
control.orientation.z = 1;
control.name = "move_y";
control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);*/


server.insert(int_marker, boost::bind(&MarkerServer::processFeedback, this, _1));

Expand Down