Skip to content

Commit 07ff48a

Browse files
author
Thomas Witte
committed
dummy_object node hinzugefügt
1 parent 6d83a0b commit 07ff48a

4 files changed

Lines changed: 179 additions & 6 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -179,7 +179,7 @@ add_library(${PROJECT_NAME}
179179
## Declare a C++ executable
180180
## With catkin_make all packages are built within a single CMake context
181181
## The recommended prefix ensures that target names across packages don't collide
182-
# add_executable(${PROJECT_NAME}_node src/interactive_script_node.cpp)
182+
add_executable(dummy_object src/dummy_object.cpp)
183183

184184
## Rename C++ executable without prefix
185185
## The above recommended prefix causes long target names, the following renames the
@@ -189,7 +189,7 @@ add_library(${PROJECT_NAME}
189189

190190
## Add cmake target dependencies of the executable
191191
## same as for the library above
192-
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
192+
add_dependencies(dummy_object ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
193193

194194
## Specify libraries to link a library or executable target against
195195
target_link_libraries(${PROJECT_NAME}
@@ -198,7 +198,12 @@ target_link_libraries(${PROJECT_NAME}
198198
MiniLua
199199
)
200200

201+
target_link_libraries(dummy_object
202+
${catkin_LIBRARIES}
203+
)
204+
201205
target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_17)
206+
target_compile_features(dummy_object PRIVATE cxx_std_17)
202207

203208
#############
204209
## Install ##
@@ -219,6 +224,10 @@ install(TARGETS ${PROJECT_NAME}
219224
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
220225
)
221226

227+
install(TARGETS dummy_object
228+
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
229+
)
230+
222231
## Mark cpp header files for installation
223232
install(DIRECTORY include/${PROJECT_NAME}/
224233
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

‎launch/start_quad_node.launch‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -63,4 +63,56 @@
6363
type="double"
6464
value="0" />
6565
</node>
66+
67+
<!-- dummy_object -->
68+
<node name="helm_blau"
69+
pkg="interactive_script"
70+
type="dummy_object"
71+
output="screen"
72+
required="true">
73+
<param name="name"
74+
type="string"
75+
value="helm_blau" />
76+
<param name="interactive_marker_topic"
77+
type="string"
78+
value="/simple_marker" />
79+
<param name="x"
80+
type="double"
81+
value="2.0" />
82+
<rosparam param="color">[0.0, 0.0, 1.0]</rosparam>
83+
</node>
84+
85+
<node name="helm_weiss"
86+
pkg="interactive_script"
87+
type="dummy_object"
88+
output="screen"
89+
required="true">
90+
<param name="name"
91+
type="string"
92+
value="helm_weiss" />
93+
<param name="interactive_marker_topic"
94+
type="string"
95+
value="/simple_marker" />
96+
<param name="x"
97+
type="double"
98+
value="3.0" />
99+
<rosparam param="color">[1.0, 1.0, 1.0]</rosparam>
100+
</node>
101+
102+
<node name="helm_gelb"
103+
pkg="interactive_script"
104+
type="dummy_object"
105+
output="screen"
106+
required="true">
107+
<param name="name"
108+
type="string"
109+
value="helm_gelb" />
110+
<param name="interactive_marker_topic"
111+
type="string"
112+
value="/simple_marker" />
113+
<param name="x"
114+
type="double"
115+
value="4.0" />
116+
<rosparam param="color">[1.0, 1.0, 0.0]</rosparam>
117+
</node>
66118
</launch>

‎src/dummy_object.cpp‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
#include <quad_common_utils/params.h>
2+
#include <quad_common_utils/geometry_msg_helper.h>
3+
4+
#include <interactive_markers/interactive_marker_server.h>
5+
#include <tf2_ros/transform_broadcaster.h>
6+
#include <visualization_msgs/Marker.h>
7+
8+
#include <ros/ros.h>
9+
#include <string>
10+
11+
PARAM_REQ(OBJECT_NAME, "~name");
12+
PARAM_REQ(COLOR, "~color");
13+
PARAM_REQ(INTERACTIVE_MARKER_TOPIC, "~interactive_marker_topic");
14+
PARAM_DEF(WORLD_FRAME, "~world_frame", "world");
15+
PARAM_DEF(OBJECT_MESH, "~mesh", "package://trajectory_server/meshes/hardhat.dae");
16+
PARAM_DEF(OBJECT_SHAPE, "~shape", visualization_msgs::Marker::MESH_RESOURCE);
17+
PARAM_DEF(POS_X, "~x", 0.0);
18+
PARAM_DEF(POS_Y, "~y", 0.0);
19+
PARAM_DEF(POS_Z, "~z", 0.0);
20+
PARAM_DEF(POS_PSI, "~psi", 0.0);
21+
22+
int main(int argc, char *argv[]) {
23+
ros::init(argc, argv, "dummy_object");
24+
25+
ros::NodeHandle nh;
26+
tf2_ros::TransformBroadcaster br;
27+
28+
LOAD_PARAM_REQ(OBJECT_NAME, std::string);
29+
LOAD_PARAM_REQ(INTERACTIVE_MARKER_TOPIC, std::string);
30+
LOAD_PARAM_REQ(COLOR, std::vector<double>);
31+
LOAD_PARAM_DEF(WORLD_FRAME, std::string);
32+
LOAD_PARAM_DEF(OBJECT_MESH, std::string);
33+
LOAD_PARAM_DEF(OBJECT_SHAPE, int);
34+
LOAD_PARAM_DEF(POS_X, double);
35+
LOAD_PARAM_DEF(POS_Y, double);
36+
LOAD_PARAM_DEF(POS_Z, double);
37+
LOAD_PARAM_DEF(POS_PSI, double);
38+
39+
interactive_markers::InteractiveMarkerServer server(INTERACTIVE_MARKER_TOPIC);
40+
41+
visualization_msgs::InteractiveMarker marker;
42+
43+
marker.header.frame_id = WORLD_FRAME;
44+
marker.header.stamp = ros::Time::now();
45+
marker.name = OBJECT_NAME;
46+
marker.pose = geometry_msgs::pose(POS_X, POS_Y, POS_Z, POS_PSI);
47+
48+
visualization_msgs::InteractiveMarkerControl control;
49+
50+
control.name = "helmet";
51+
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE;
52+
control.always_visible = true;
53+
control.orientation.w = 1;
54+
control.orientation.x = 0;
55+
control.orientation.y = 1;
56+
control.orientation.z = 0;
57+
visualization_msgs::Marker m;
58+
59+
m.scale = geometry_msgs::vec3(1, 1, 1);
60+
m.mesh_resource = OBJECT_MESH;
61+
m.type = OBJECT_SHAPE;
62+
m.header = marker.header;
63+
m.ns = "object_" + OBJECT_NAME;
64+
m.id = 0;
65+
m.action = visualization_msgs::Marker::ADD;
66+
m.color.r = COLOR[0];
67+
m.color.g = COLOR[1];
68+
m.color.b = COLOR[2];
69+
m.color.a = 1.0;
70+
m.pose = geometry_msgs::pose(POS_X, POS_Y, POS_Z, POS_PSI + M_PI/2);
71+
72+
control.markers.push_back(m);
73+
marker.controls.push_back(control);
74+
75+
server.insert(marker, [&](const auto& feedback) {
76+
POS_PSI = geometry_msgs::yaw(feedback->pose.orientation);
77+
POS_X = feedback->pose.position.x;
78+
POS_Y = feedback->pose.position.y;
79+
POS_Z = feedback->pose.position.z;
80+
});
81+
82+
server.applyChanges();
83+
84+
ros::Timer timer = nh.createTimer(ros::Duration(1.0/30), [&](const ros::TimerEvent&) {
85+
geometry_msgs::TransformStamped transform;
86+
transform.header.frame_id = WORLD_FRAME;
87+
transform.child_frame_id = OBJECT_NAME;
88+
transform.header.stamp = ros::Time::now();
89+
transform.transform.translation.x = POS_X;
90+
transform.transform.translation.y = POS_Y;
91+
transform.transform.translation.z = POS_Z;
92+
transform.transform.rotation = geometry_msgs::quaternion(0.0, 0.0, POS_PSI);
93+
br.sendTransform(transform);
94+
});
95+
96+
ros::spin();
97+
98+
return 0;
99+
}

‎src/interactive_script_plugin/marker_interface.cpp‎

Lines changed: 17 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -192,7 +192,7 @@ void MarkerInterface::addPose(double x, double y, double z, double psi,
192192
int_marker.pose.orientation = geometry_msgs::quaternion();
193193

194194
// create a grey box marker
195-
visualization_msgs::Marker box_marker;
195+
/*visualization_msgs::Marker box_marker;
196196
box_marker.type = visualization_msgs::Marker::CUBE;
197197
box_marker.scale.x = 0.3;
198198
box_marker.scale.y = 0.3;
@@ -212,13 +212,26 @@ void MarkerInterface::addPose(double x, double y, double z, double psi,
212212
arrow_marker.color.g = 1.0;
213213
arrow_marker.color.b = 1.0;
214214
arrow_marker.color.a = 1.0;
215-
arrow_marker.pose.orientation = geometry_msgs::quaternion(0,0,psi);
215+
arrow_marker.pose.orientation = geometry_msgs::quaternion(0,0,psi);*/
216+
217+
visualization_msgs::Marker quad_marker;
218+
quad_marker.type = visualization_msgs::Marker::MESH_RESOURCE;
219+
quad_marker.mesh_resource = "package://trajectory_server/meshes/bebop.dae";
220+
quad_marker.scale.x = 0.002;
221+
quad_marker.scale.y = 0.002;
222+
quad_marker.scale.z = 0.002;
223+
quad_marker.color.r = 1.0;
224+
quad_marker.color.g = 1.0;
225+
quad_marker.color.b = 1.0;
226+
quad_marker.color.a = 1.0;
227+
quad_marker.pose.orientation = geometry_msgs::quaternion(0,0,psi);
216228

217229
// create a non-interactive control which contains the box
218230
visualization_msgs::InteractiveMarkerControl box_control;
219231
box_control.always_visible = true;
220-
box_control.markers.push_back( box_marker );
221-
box_control.markers.push_back( arrow_marker );
232+
//box_control.markers.push_back( box_marker );
233+
//box_control.markers.push_back( arrow_marker );
234+
box_control.markers.push_back( quad_marker );
222235
box_control.name = "clicked";
223236
box_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
224237
//box_control.orientation = geometry_msgs::quaternion(0,0,psi);

0 commit comments

Comments
 (0)