Skip to content
Snippets Groups Projects
Commit 9869b41a authored by Johannes Mey's avatar Johannes Mey
Browse files

change messaged to stamped messages, add initial version of plotter

parent c7079352
Branches
No related tags found
No related merge requests found
...@@ -80,6 +80,8 @@ add_message_files( ...@@ -80,6 +80,8 @@ add_message_files(
FILES FILES
Trajectory.msg Trajectory.msg
Waypoint.msg Waypoint.msg
StampedVelocity.msg
StampedInZone.msg
) )
## Generate services in the 'srv' folder ## Generate services in the 'srv' folder
...@@ -204,4 +206,8 @@ target_link_libraries(${PROJECT_NAME}_safety_zone_spawner ${catkin_LIBRARIES}) ...@@ -204,4 +206,8 @@ target_link_libraries(${PROJECT_NAME}_safety_zone_spawner ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_safety_zone_monitor ${catkin_LIBRARIES}) target_link_libraries(${PROJECT_NAME}_safety_zone_monitor ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_acceleration_publisher ${catkin_LIBRARIES}) target_link_libraries(${PROJECT_NAME}_acceleration_publisher ${catkin_LIBRARIES})
\ No newline at end of file
catkin_install_python(PROGRAMS scripts/plotter.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
\ No newline at end of file
...@@ -13,6 +13,9 @@ ...@@ -13,6 +13,9 @@
<node name="safety_zone_monitor" pkg="panda_mqtt_connector" type="safety_zone_monitor" respawn="false" output="screen"/> <node name="safety_zone_monitor" pkg="panda_mqtt_connector" type="safety_zone_monitor" respawn="false" output="screen"/>
<node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/> <node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
<node name="acceleration_publisher" pkg="panda_mqtt_connector" type="acceleration_publisher" respawn="false" output="screen"/> <node name="acceleration_publisher" pkg="panda_mqtt_connector" type="acceleration_publisher" respawn="false" output="screen"/>
<node name="transform-riz" pkg="topic_tools" type="transform" args="/panda_mqtt_connector/robot_in_zone /panda_mqtt_connector/riz std_msgs/Float64 -- 'm.data*-1'" /> <node name="plotter" pkg="panda_mqtt_connector" type="plotter.py" respawn="false" output="screen"/>
<node name="rqt_plot" pkg="rqt_plot" type="rqt_plot" respawn="false" output="screen" args="/gazebo/link_states/pose[10]/position/x:y:z /panda_mqtt_connector/velocity/data /panda_mqtt_connector/max_velocity/data /panda_mqtt_connector/riz" /> <node name="transform_riz" pkg="topic_tools" type="transform" args="--wait-for-start /panda_mqtt_connector/robot_in_zone /panda_mqtt_connector/robot_in_zone_number std_msgs/Float64 -- 'm.zone*-1'" />
<node name="rqt_plot" pkg="rqt_plot" type="rqt_plot" respawn="false" output="screen"
args="/gazebo/link_states/pose[10]/position/x:y:z /panda_mqtt_connector/velocity /panda_mqtt_connector/max_velocity /panda_mqtt_connector/robot_in_zone_number"
/>
</launch> </launch>
# information if robot is in zone with a time stamp
Header header
bool zone
\ No newline at end of file
# a velocity with a time stamp
Header header
float64 velocity
\ No newline at end of file
#!/usr/bin/env python
from matplotlib import pyplot as plt
import rospy
from panda_mqtt_connector.msg import StampedVelocity
def plot_x(msg):
plt.plot(msg.header.stamp.to_sec(), msg.velocity, '*')
plt.axis("equal")
plt.draw()
plt.pause(0.00000000001)
if __name__ == '__main__':
rospy.init_node("plotter")
rospy.Subscriber("panda_mqtt_connector/velocity", StampedVelocity, plot_x)
plt.ion()
plt.show()
rospy.spin()
...@@ -7,7 +7,7 @@ ...@@ -7,7 +7,7 @@
#include <geometry_msgs/Point.h> #include <geometry_msgs/Point.h>
#include <gazebo_msgs/LinkStates.h> #include <gazebo_msgs/LinkStates.h>
#include <std_msgs/Bool.h> #include <panda_mqtt_connector/StampedInZone.h>
#include "ros/ros.h" #include "ros/ros.h"
...@@ -52,8 +52,6 @@ public: ...@@ -52,8 +52,6 @@ public:
counter++; counter++;
// ROS_ERROR_STREAM("initialized " << initialized << " result " << res << " counter " << counter);
std::vector<std::string> names{msg.name}; std::vector<std::string> names{msg.name};
for (int link_number = 0; link_number < names.size(); ++link_number) { for (int link_number = 0; link_number < names.size(); ++link_number) {
...@@ -64,9 +62,10 @@ public: ...@@ -64,9 +62,10 @@ public:
if (!res || !initialized) { if (!res || !initialized) {
initialized = true; initialized = true;
res = true; res = true;
std_msgs::Bool result; panda_mqtt_connector::StampedInZone result;
result.data = true; result.zone = true;
ROS_ERROR_STREAM("entered zone [" << zone.zone.x << "," << zone.zone.y << "] in run " << counter); result.header.stamp = ros::Time::now();
ROS_INFO_STREAM("entered zone [" << zone.zone.x << "," << zone.zone.y << "] in run " << counter);
publisher->publish(result); publisher->publish(result);
} }
return; return;
...@@ -76,11 +75,12 @@ public: ...@@ -76,11 +75,12 @@ public:
} }
// no part of the robot is in a zone // no part of the robot is in a zone
if (res || !initialized) { if (res || !initialized) {
ROS_ERROR_STREAM("left zone in run " << counter); ROS_INFO_STREAM("left zone in run " << counter);
initialized = true; initialized = true;
res = false; res = false;
std_msgs::Bool result; panda_mqtt_connector::StampedInZone result;
result.data = false; result.zone = false;
result.header.stamp = ros::Time::now();
publisher->publish(result); publisher->publish(result);
} }
} }
......
...@@ -7,10 +7,8 @@ ...@@ -7,10 +7,8 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h> #include <panda_mqtt_connector/StampedVelocity.h>
#include <std_msgs/Float64.h>
namespace acceleration_publisher { namespace acceleration_publisher {
} }
...@@ -25,7 +23,7 @@ int main(int argc, char **argv) { ...@@ -25,7 +23,7 @@ int main(int argc, char **argv) {
ros::init(argc, argv, "acceleration_publisher"); ros::init(argc, argv, "acceleration_publisher");
ros::NodeHandle n("panda_mqtt_connector"); ros::NodeHandle n("panda_mqtt_connector");
ros::Publisher vel = n.advertise<std_msgs::Float64>("velocity", 10); ros::Publisher vel = n.advertise<panda_mqtt_connector::StampedVelocity>("velocity", 10);
tf2_ros::Buffer tfBuffer; tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer); tf2_ros::TransformListener tfListener(tfBuffer);
...@@ -46,21 +44,22 @@ int main(int argc, char **argv) { ...@@ -46,21 +44,22 @@ int main(int argc, char **argv) {
"panda_hand", past, "panda_hand", past,
"world", ros::Duration(1.0)); "world", ros::Duration(1.0));
panda_mqtt_connector::StampedVelocity msg;
double dx = transformStamped.transform.translation.x;
double dy = transformStamped.transform.translation.y;
double dz = transformStamped.transform.translation.z;
double velocity = sqrt(dx*dx + dy*dz + dz*dz)/dt.toSec();
msg.header.stamp = now - (dt * .5);
msg.velocity = velocity;
vel.publish(msg);
} catch (tf2::TransformException &ex) { } catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what()); ROS_WARN("%s", ex.what());
ros::Duration(1.0).sleep(); ros::Duration(1.0).sleep();
continue; continue;
} }
std_msgs::Float64 msg;
double dx = transformStamped.transform.translation.x;
double dy = transformStamped.transform.translation.y;
double dz = transformStamped.transform.translation.z;
double velocity = sqrt(dx*dx + dy*dz + dz*dz)/dt.toSec();
msg.data = velocity;
vel.publish(msg);
} }
return 0; return 0;
......
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <shape_msgs/SolidPrimitive.h> #include <shape_msgs/SolidPrimitive.h>
#include <geometry_msgs/Pose.h> #include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h> #include <panda_mqtt_connector/StampedInZone.h>
#include "SafetyZoneMonitor.h" #include "SafetyZoneMonitor.h"
namespace safety_zone_monitor { namespace safety_zone_monitor {
...@@ -73,7 +73,7 @@ int main(int argc, char **argv) { ...@@ -73,7 +73,7 @@ int main(int argc, char **argv) {
exit(-1); exit(-1);
} }
ros::Publisher zoneStatePublisher = n.advertise<std_msgs::Bool>("robot_in_zone", 1000); ros::Publisher zoneStatePublisher = n.advertise<panda_mqtt_connector::StampedInZone>("robot_in_zone", 1000);
SafetyZoneMonitor monitor{size, zoneStatePublisher}; SafetyZoneMonitor monitor{size, zoneStatePublisher};
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment