Skip to content

Commit 8e481d6

Browse files
author
Thomas Witte
committed
Merge branch 'master' of spgit.informatik.uni-ulm.de:quadrocopter/interactive_script
2 parents 8ed2711 + 976a015 commit 8e481d6

3 files changed

Lines changed: 34 additions & 13 deletions

File tree

‎interactive_script/src/interactive_script_plugin/builtins.cpp‎

Lines changed: 22 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -94,10 +94,16 @@ void VisualizationInterpreter::populate_visualization_env(Environment& env, LuaP
9494
env.assign("print", make_shared<lua::rt::cfunction>([this](const lua::rt::vallist& args) -> cfunction::result {
9595
stringstream ss;
9696
for (int i = 0; i < static_cast<int>(args.size()) - 1; ++i) {
97-
ss << args[static_cast<unsigned>(i)].to_string() << "\t";
97+
if (args[static_cast<unsigned>(i)].istable())
98+
ss << args[static_cast<unsigned>(i)].literal() << "\t";
99+
else
100+
ss << args[static_cast<unsigned>(i)].to_string() << "\t";
98101
}
99102
if (!args.empty()) {
100-
ss << args.back().to_string();
103+
if (args.back().istable())
104+
ss << args.back().literal();
105+
else
106+
ss << args.back().to_string();
101107
}
102108
signal.appendTerminal(QString::fromStdString(ss.str()));
103109
return {};
@@ -115,13 +121,13 @@ void VisualizationInterpreter::populate_visualization_env(Environment& env, LuaP
115121
marker.addPose(get<double>(args[0]),
116122
get<double>(args[1]),
117123
get<double>(args[2]),
118-
get<double>(args[3]),
124+
get<double>(args[3]) * 2 * M_PI / 360.0,
119125
args[0].source.get() && settings.move_markers,
120126
args[1].source.get() && settings.move_markers,
121127
args[2].source.get() && settings.move_markers,
122128
args[3].source.get() && settings.move_markers,
123129
[x_source = args[0].source, y_source = args[1].source, z_source = args[2].source, psi_source = args[3].source,
124-
this, settings, tokens = parser.tokens, original_psi = get<double>(args[3])](const auto& feedback) mutable {
130+
this, settings, tokens = parser.tokens, original_psi = get<double>(args[3])*M_PI/180.0](const auto& feedback) mutable {
125131
if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK && settings.click_for_dependency_trace) {
126132
vector<LuaToken> tokens;
127133
signal.removeFormatting();
@@ -161,7 +167,7 @@ void VisualizationInterpreter::populate_visualization_env(Environment& env, LuaP
161167
changes->changes.push_back(*change);
162168
}
163169
if (psi_source && feedback->control_name == "move_psi") {
164-
if (const auto& change = psi_source->forceValue(fmod(yaw(feedback->pose.orientation) + original_psi, 2*M_PI)))
170+
if (const auto& change = psi_source->forceValue(fmod(yaw(feedback->pose.orientation) + original_psi, 2*M_PI) * 180.0/M_PI))
165171
changes->changes.push_back(*change);
166172
}
167173

@@ -281,7 +287,7 @@ void VisualizationInterpreter::populate_visualization_env(Environment& env, LuaP
281287
t["x"] = p ? p->position.x : 0;
282288
t["y"] = p ? p->position.y : 0;
283289
t["z"] = p ? p->position.z : 0;
284-
t["psi"] = p ? yaw(p->orientation) : 0;
290+
t["psi"] = p ? yaw(p->orientation) * 180.0/M_PI : 0;
285291

286292
return {result};
287293
}
@@ -507,12 +513,17 @@ void LiveScriptInterpreter::populate_live_env(lua::rt::Environment &env, const A
507513

508514
stringstream ss;
509515
for (int i = 0; i < static_cast<int>(args.size()) - 1; ++i) {
510-
ss << args[static_cast<unsigned>(i)].to_string() << "\t";
516+
if (args[static_cast<unsigned>(i)].istable())
517+
ss << args[static_cast<unsigned>(i)].literal() << "\t";
518+
else
519+
ss << args[static_cast<unsigned>(i)].to_string() << "\t";
511520
}
512521
if (!args.empty()) {
513-
ss << args.back().to_string();
522+
if (args.back().istable())
523+
ss << args.back().literal();
524+
else
525+
ss << args.back().to_string();
514526
}
515-
cout << ss.str() << endl;
516527
signal.appendTerminal(QString::fromStdString(ss.str()));
517528
return {};
518529
}), false);
@@ -558,7 +569,7 @@ void LiveScriptInterpreter::populate_live_env(lua::rt::Environment &env, const A
558569
return string {"Script execution cancelled!"};
559570

560571
if (args.size() == 4 && args[0].isnumber() && args[1].isnumber() && args[2].isnumber() && args[3].isnumber()) {
561-
quad.send_new_waypoint(geometry_msgs::pose(get<double>(args[0]), get<double>(args[1]), get<double>(args[2]), get<double>(args[3])));
572+
quad.send_new_waypoint(geometry_msgs::pose(get<double>(args[0]), get<double>(args[1]), get<double>(args[2]), get<double>(args[3])/360.0 * 2 * M_PI));
562573

563574
return {};
564575
}
@@ -596,7 +607,7 @@ void LiveScriptInterpreter::populate_live_env(lua::rt::Environment &env, const A
596607
(*t)["x"] = p.position.x;
597608
(*t)["y"] = p.position.y;
598609
(*t)["z"] = p.position.x;
599-
(*t)["psi"] = yaw(p.orientation);
610+
(*t)["psi"] = yaw(p.orientation) * 180.0/M_PI;
600611

601612
return {t};
602613
}

‎interactive_script/src/interactive_script_plugin/interactive_script_plugin.cpp‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -264,8 +264,10 @@ void InteractiveScriptGui::on_save() {
264264
}
265265
}*/
266266

267-
Filename f {"/data/save_" + std::to_string(time(nullptr)) + ".xml", Filename::XML};
267+
Filename f {"/data/save_" + std::to_string(time(nullptr)) + ".manual.xml", Filename::XML};
268268
save_file(f, ui_.blockly_widget->xml().toStdString());
269+
ui_.terminal->setPlainText(QString::fromStdString(f.name + " saved"));
270+
ROS_INFO_STREAM(f.name << " saved");
269271
}
270272

271273
void InteractiveScriptGui::on_load() {
@@ -283,9 +285,11 @@ void InteractiveScriptGui::on_load() {
283285
}
284286
}*/
285287

286-
Filename f {QDir("/data", "save_*.xml").entryList().back().toStdString(), Filename::XML};
288+
Filename f {"/data/" + QDir("/data", "*.manual.xml").entryList().back().toStdString(), Filename::XML};
287289
ui_.blockly_widget->loadXml(QString::fromStdString(load_file(f)));
288290
ui_.tabWidget->setCurrentWidget(ui_.blockly_tab);
291+
ui_.terminal->setPlainText(QString::fromStdString(f.name + " loaded"));
292+
ROS_INFO_STREAM(f.name << " loaded");
289293
}
290294

291295
bool InteractiveScriptGui::hasConfiguration() const {

‎interactive_script/src/interactive_script_plugin/marker_interface.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -266,6 +266,8 @@ void MarkerInterface::addPose(double x, double y, double z, double psi,
266266
control.orientation.x = 0;
267267
control.orientation.y = 0;
268268
control.orientation.z = 1;
269+
control.orientation_mode =
270+
visualization_msgs::InteractiveMarkerControl::INHERIT;
269271
control.interaction_mode =
270272
visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
271273

@@ -283,6 +285,8 @@ void MarkerInterface::addPose(double x, double y, double z, double psi,
283285
control.orientation.x = 0;
284286
control.orientation.y = 1;
285287
control.orientation.z = 0;
288+
control.orientation_mode =
289+
visualization_msgs::InteractiveMarkerControl::INHERIT;
286290
control.interaction_mode =
287291
visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
288292

@@ -300,6 +304,8 @@ void MarkerInterface::addPose(double x, double y, double z, double psi,
300304
control.orientation.x = 0;
301305
control.orientation.y = 1;
302306
control.orientation.z = 0;
307+
control.orientation_mode =
308+
visualization_msgs::InteractiveMarkerControl::INHERIT;
303309
control.interaction_mode =
304310
visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
305311

0 commit comments

Comments
 (0)