actionlib support in roscs?
Hi, Does ROSCS supports actionlib and also using custom messages?
ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
Hi, Does ROSCS supports actionlib and also using custom messages?
Reading the documentation at http://wiki.ros.org/roscs appears to support custom messages but there does not appear to be actionlib support.
Tully is right. You need to send the msgs, which are autogenerated by actionlib and then you are missing the support of actionlib server and/or client. Here is one messy little class that did the job for us so far.
using System;
using System.Threading;
using System.Collections;
using System.Collections.Generic;
using RosCS;
using RosCS.geometry_msgs;
using RosCS.nav_msgs;
using RosCS.tmc200_msgs;
using RosCS.actionlib_msgs;
using RosCS.move_base_msgs;
using RosCS.Services;
using RosCS.Services.nav_msgs;
using Castor;
public class ArmHelper
{
public static Pose PointOfInterest { get; private set; }
public static bool NewPOI { get; set; }
public static bool ExplorationDone { get; set; }
public static GoalID CurrentGoalID{get; private set;}
public static GoalStatusArray GoalStatus {get; private set;}
public static MoveBaseActionFeedback GoalFeedback {get; private set;}
public static MoveBaseActionResult GoalResult {get; set;}
public static bool MotionEnabled { get; set; }
public static string MotionType { get; set; }
protected static Node rosNode;
protected static int id = 0;
protected static MoveBaseActionGoal MoveBaseGoal{get; private set;}
protected static SystemConfig sc = SystemConfig.LocalInstance;
protected static ArmHelper instance = null;
protected static object lockobject = new object();
public static ArmHelper Get() {
if(instance == null) {
lock(lockobject) {
if(instance == null) {
instance = new ArmHelper();
//Thread.MemoryBarrier();
}
}
}
return instance;
}
protected static Publisher motionPub;
protected static Publisher goalPositionPub;
protected static Publisher mBaseGoalPositionPub;
protected static Publisher mBaseCancelGoalPub;
protected static string GenerateUniqueId()
{
id++;
return "goal_" + id;
}
protected static GoalID GenerateActionLibGoalId()
{
CurrentGoalID = new GoalID();
CurrentGoalID.Stamp = RosSharp.Now();
CurrentGoalID.Id = GenerateUniqueId();
return CurrentGoalID;
}
public static void RosInit()
{
if(rosNode!= null) return;
rosNode = Node.MainNode;
ExplorationDone = false;
MotionEnabled = false;
MotionType = sc["ImperaHelper"].GetString("ImperaHelper.PathHelper.MotionType");
mBaseCancelGoalPub = new Publisher(Node.MainNode, "/move_base/cancel", GoalID.TypeId, 1);
goalPositionPub = new Publisher(Node.MainNode, "/move_base_simple/goal", PoseStamped.TypeId, 1);
mBaseGoalPositionPub = new Publisher(Node.MainNode, "/move_base/goal", MoveBaseActionGoal.TypeId, 1);
motionPub = new Publisher(Node.MainNode,"MotionControl",MotionControl.TypeId,1);
Node.MainNode.Subscribe("/move_base/status", OnGoalStatus);
Node.MainNode.Subscribe("/move_base/feedback", OnGoalFeedback);
Node.MainNode.Subscribe("/move_base/result", OnGoalResult);
Node.MainNode.Subscribe("/PointOfInterest", OnPOI);
Node.MainNode.Subscribe("/cmd_vel", OnTwist);
}
protected ArmHelper()
{
if(rosNode == null) {
RosInit();
}
}
protected static void OnTwist(Twist tc)
{
if (!MotionEnabled)
return;
if(MotionType.Equals("tmc"))
{
MotionControl mc = new MotionControl();
mc.Motion.Angle = Math.Atan2(tc.Linear.Y, tc.Linear.X);
mc.Motion.Translation = 1000*Math.Sqrt(tc.Linear.X * tc.Linear.X + tc.Linear.Y * tc.Linear.Y);
mc.Motion.Rotation = tc.Angular.Z;
//Console.WriteLine("Velocity : " + mc.Motion.Translation + " Turn : " + mc.Motion.Rotation + " Angle : " + mc.Motion.Angle);
motionPub.Send(mc);
}
}
protected static void OnPOI(PoseStamped ps) {
NewPOI = true;
PointOfInterest = ps.Pose;
Console.WriteLine("PH: Got new POI");
}
protected static void OnGoalStatus(GoalStatusArray gStatus)
{
GoalStatus = new GoalStatusArray();
GoalStatus.Header = gStatus.Header;
GoalStatus.Status_list = gStatus.Status_list;
//Console.WriteLine("Total Goal Count: "+GoalStatus.Status_list.Count);
}
protected static void OnGoalFeedback(MoveBaseActionFeedback feedback)
{
GoalFeedback = new MoveBaseActionFeedback();
GoalFeedback.Feedback = feedback.Feedback;
GoalFeedback.Header = feedback.Header;
GoalFeedback.Status = feedback.Status;
//Console.WriteLine("Feedback: GoalID: "+GoalFeedback.Status.Goal_id.Id+ " Status: "+GoalFeedback.Status.Status);
}
protected static void OnGoalResult(MoveBaseActionResult result)
{
GoalResult = new MoveBaseActionResult();
GoalResult.Header = result.Header;
GoalResult.Result = result.Result;
GoalResult.Status = result.Status;
Console.WriteLine("Result: GoalID: "+GoalResult.Status.Goal_id.Id ...
(more)Asked: 2012-07-10 22:58:31 -0500
Seen: 435 times
Last updated: Dec 15 '13
Interrupting a joint trajectory
rosjava actionlib waitForResult() function [closed]
Python SimpleActionServer stops unexpectedly (deadlock!)
Actionlib connection_monitor.cpp fails when trying to compile ROS Medlodic from source
How do package.xml and CMakeLists.txt work
Custom action servers run in a launch file problem
SMACH sequences from configuration file
Where is documentation for Actionlib Client?
Multiple action clients to one server and correct preemption notifications